diff --git a/common/params.cc b/common/params.cc index a65d522ae..7ef44cbf5 100644 --- a/common/params.cc +++ b/common/params.cc @@ -240,6 +240,7 @@ std::unordered_map keys = { {"AutomaticallyDownloadModels", PERSISTENT}, {"AutomaticUpdates", PERSISTENT}, {"AvailableModelNames", PERSISTENT}, + {"AvailableModelSeries", PERSISTENT}, {"AvailableModels", PERSISTENT}, {"BigMap", PERSISTENT}, {"BlacklistedModels", PERSISTENT}, @@ -398,6 +399,7 @@ std::unordered_map keys = { {"MinimumBackupSize", PERSISTENT}, {"MinimumLaneChangeSpeed", PERSISTENT}, {"Model", PERSISTENT}, + {"ModelVersion", PERSISTENT}, {"ModelDownloadProgress", CLEAR_ON_MANAGER_START}, {"ModelDrivesAndScores", PERSISTENT}, {"ModelRandomizer", PERSISTENT}, @@ -576,6 +578,7 @@ std::unordered_map keys = { {"WarningSoftVolume", PERSISTENT}, {"WheelIcon", PERSISTENT}, {"WheelSpeed", PERSISTENT}, + {"StopDistance", PERSISTENT}, {"WheelToDownload", CLEAR_ON_MANAGER_START}, }; diff --git a/frogpilot/assets/download_functions.py b/frogpilot/assets/download_functions.py index 5abed191f..b93a7c68e 100644 --- a/frogpilot/assets/download_functions.py +++ b/frogpilot/assets/download_functions.py @@ -6,14 +6,13 @@ from datetime import datetime from pathlib import Path from openpilot.frogpilot.common.frogpilot_utilities import delete_file, is_url_pingable -from openpilot.frogpilot.common.frogpilot_variables import RESOURCES_REPO, params_memory -GITHUB_URL = f"https://raw.githubusercontent.com/{RESOURCES_REPO}" -GITLAB_URL = f"https://gitlab.com/{RESOURCES_REPO}/-/raw" +GITHUB_URL = "https://raw.githubusercontent.com/firestar5683/StarPilot-Resources" +GITLAB_URL = "https://gitlab.com/firestar5683/FrogPilot-Resources/-/raw" -def check_github_rate_limit(session): +def check_github_rate_limit(): try: - response = session.get("https://api.github.com/rate_limit", timeout=10) + response = requests.get("https://api.github.com/rate_limit") response.raise_for_status() rate_limit_info = response.json() @@ -26,70 +25,66 @@ def check_github_rate_limit(session): print("GitHub rate limit reached") print(f"GitHub Rate Limit Resets At (UTC): {reset_time}") return False - except requests.exceptions.RequestException as exception: - print(f"Error checking GitHub rate limit: {exception}") + except requests.exceptions.RequestException as error: + print(f"Error checking GitHub rate limit: {error}") return False -def download_file(cancel_param, destination, progress_param, url, download_param, session, offset_bytes=0, total_bytes=0): +def download_file(cancel_param, destination, progress_param, url, download_param, params_memory): try: destination.parent.mkdir(parents=True, exist_ok=True) - total_size = get_remote_file_size(url, session) + total_size = get_remote_file_size(url) if total_size == 0: if not url.endswith(".gif"): - handle_error(None, "Download invalid...", "Download invalid...", download_param, progress_param) + handle_error(None, "Download invalid...", "Download invalid...", download_param, progress_param, params_memory) return - with session.get(url, stream=True, timeout=10) as response: + with requests.get(url, stream=True, timeout=10) as response: response.raise_for_status() - with tempfile.NamedTemporaryFile(delete=False, dir=destination.parent) as temp_file: + with tempfile.NamedTemporaryFile(dir=destination.parent, delete=False) as temp_file: temp_file_path = Path(temp_file.name) downloaded_size = 0 for chunk in response.iter_content(chunk_size=16384): if params_memory.get_bool(cancel_param): temp_file_path.unlink(missing_ok=True) - handle_error(None, "Download cancelled...", "Download cancelled...", download_param, progress_param) + handle_error(None, "Download cancelled...", "Download cancelled...", download_param, progress_param, params_memory) return if chunk: temp_file.write(chunk) downloaded_size += len(chunk) - if total_bytes: - overall_progress = (offset_bytes + downloaded_size) / total_bytes * 100 - else: - overall_progress = downloaded_size / total_size * 100 - - if overall_progress != 100: - params_memory.put(progress_param, f"{overall_progress:.0f}%") + progress = (downloaded_size / total_size) * 100 + if progress != 100: + params_memory.put(progress_param, f"{progress:.0f}%") else: params_memory.put(progress_param, "Verifying authenticity...") temp_file_path.rename(destination) - except Exception as exception: - handle_request_error(exception, destination, download_param, progress_param) + except Exception as error: + handle_request_error(error, destination, download_param, progress_param, params_memory) -def get_remote_file_size(url, session): +def get_remote_file_size(url): try: - response = session.head(url, headers={"Accept-Encoding": "identity"}, timeout=10) + response = requests.head(url, headers={"Accept-Encoding": "identity"}, timeout=10) response.raise_for_status() return int(response.headers.get("Content-Length", 0)) - except Exception as exception: - handle_request_error(exception, None, None, None) + except Exception as error: + handle_request_error(error, None, None, None, None) return 0 -def get_repository_url(session): +def get_repository_url(): if is_url_pingable("https://github.com"): - if check_github_rate_limit(session): + if check_github_rate_limit(): return GITHUB_URL if is_url_pingable("https://gitlab.com"): return GITLAB_URL return None -def handle_error(destination, error_message, error, download_param, progress_param): +def handle_error(destination, error_message, error, download_param, progress_param, params_memory): if destination: delete_file(destination) @@ -98,19 +93,19 @@ def handle_error(destination, error_message, error, download_param, progress_par params_memory.put(progress_param, error_message) params_memory.remove(download_param) -def handle_request_error(error, destination, download_param, progress_param): +def handle_request_error(error, destination, download_param, progress_param, params_memory): error_map = { - requests.exceptions.ConnectionError: "Connection dropped", - requests.exceptions.HTTPError: lambda error: f"Server error ({error.response.status_code})" if error and getattr(error, "response", None) else "Server error", - requests.exceptions.RequestException: "Network request error. Check connection", - requests.exceptions.Timeout: "Download timed out", + requests.ConnectionError: "Connection dropped", + requests.HTTPError: lambda error: f"Server error ({error.response.status_code})" if error.response else "Server error", + requests.RequestException: "Network request error. Check connection", + requests.Timeout: "Download timed out" } error_message = error_map.get(type(error), "Unexpected error") - handle_error(destination, f"Failed: {error_message}", error, download_param, progress_param) + handle_error(destination, f"Failed: {error_message}", error, download_param, progress_param, params_memory) -def verify_download(file_path, url, session): - remote_file_size = get_remote_file_size(url, session) +def verify_download(file_path, url): + remote_file_size = get_remote_file_size(url) if remote_file_size == 0: print(f"Error fetching remote size for {file_path}") diff --git a/frogpilot/assets/model_manager.py b/frogpilot/assets/model_manager.py index c0500c7c1..18429cbd1 100644 --- a/frogpilot/assets/model_manager.py +++ b/frogpilot/assets/model_manager.py @@ -5,59 +5,230 @@ import requests import shutil import time import urllib.parse +import urllib.request from pathlib import Path -from urllib.parse import quote_plus -from openpilot.common.basedir import BASEDIR -from openpilot.frogpilot.assets.download_functions import GITLAB_URL, download_file, get_remote_file_size, get_repository_url, handle_error, handle_request_error, verify_download -from openpilot.frogpilot.common.frogpilot_utilities import delete_file, extract_tar, load_json_file, update_json_file -from openpilot.frogpilot.common.frogpilot_variables import DEFAULT_MODEL, MODELS_PATH, RESOURCES_REPO, TINYGRAD_FILES, params, params_default, params_memory, update_frogpilot_toggles +from openpilot.frogpilot.assets.download_functions import GITLAB_URL, download_file, get_repository_url, handle_error, handle_request_error, verify_download +from openpilot.frogpilot.common.frogpilot_utilities import delete_file +from openpilot.frogpilot.common.frogpilot_variables import DEFAULT_MODEL, MODELS_PATH, params, params_default, params_memory -VERSION = "v16" -VERSION_PATH = MODELS_PATH / "model_version" +VERSION = "v20" CANCEL_DOWNLOAD_PARAM = "CancelModelDownload" DOWNLOAD_PROGRESS_PARAM = "ModelDownloadProgress" MODEL_DOWNLOAD_PARAM = "ModelToDownload" MODEL_DOWNLOAD_ALL_PARAM = "DownloadAllModels" -UPDATE_TINYGRAD_PARAM = "UpdateTinygrad" - -DEFAULT_TINYGRAD_SIZE = 87746736 -TAR_FILE_NAME = f"Tinygrad_{VERSION}.tar.gz" - -TINYGRAD_MODELD_PATH = Path(BASEDIR) / "frogpilot/tinygrad_modeld" -TINYGRAD_REPO_PATH = Path(BASEDIR) / "tinygrad_repo" class ModelManager: - def __init__(self, boot_run=False): + def __init__(self): + self.available_models = (params.get("AvailableModels", encoding="utf-8") or "").split(",") + self.model_versions = (params.get("ModelVersions", encoding="utf-8") or "").split(",") + self.model_series = (params.get("AvailableModelSeries", encoding="utf-8") or "").split(",") + + self.downloading_model = False - self.available_models = (params.get("AvailableModels", encoding="utf-8") or "").split(",") - self.available_model_names = (params.get("AvailableModelNames", encoding="utf-8") or "").split(",") - self.model_versions = (params.get("ModelVersions", encoding="utf-8") or "").split(",") + @staticmethod + def fetch_models(url): + try: + with urllib.request.urlopen(url, timeout=10) as response: + return json.loads(response.read().decode("utf-8"))["models"] + except Exception as error: + handle_request_error(error, None, None, None, None) + return [] - self.model_sizes_path = MODELS_PATH / "model_sizes.json" - self.tinygrad_sizes_path = MODELS_PATH / "tinygrad_sizes.json" + @staticmethod + def fetch_all_model_sizes(repo_url): + project_path = "firestar5683/StarPilot-Resources" + branch = "Models" - self.model_sizes = load_json_file(self.model_sizes_path) - self.tinygrad_sizes = load_json_file(self.tinygrad_sizes_path) + if "github" in repo_url: + api_url = f"https://api.github.com/repos/{project_path}/contents?ref={branch}" + elif "gitlab" in repo_url: + api_url = f"https://gitlab.com/api/v4/projects/{urllib.parse.quote_plus(project_path)}/repository/tree?ref={branch}" + else: + return {} - self.session = requests.Session() - self.session.headers.update({"Accept-Language": "en"}) - self.session.headers.update({"User-Agent": "frogpilot-model-downloader/1.0 (https://github.com/FrogAi/FrogPilot)"}) + try: + response = requests.get(api_url) + response.raise_for_status() + model_files = [file for file in response.json() if "." in file["name"]] - if boot_run: - self.copy_default_model() - self.validate_models() + if "gitlab" in repo_url: + model_sizes = {} + for file in model_files: + file_path = file["path"] + metadata_url = f"https://gitlab.com/api/v4/projects/{urllib.parse.quote_plus(project_path)}/repository/files/{urllib.parse.quote_plus(file_path)}/raw?ref={branch}" + metadata_response = requests.head(metadata_url) + metadata_response.raise_for_status() + model_sizes[file["name"].rsplit(".", 1)[0]] = int(metadata_response.headers.get("content-length", 0)) + return model_sizes + else: + return {file["name"].rsplit(".", 1)[0]: file["size"] for file in model_files if "size" in file} + + except Exception as error: + handle_request_error(f"Failed to fetch model sizes from {'GitHub' if 'github' in repo_url else 'GitLab'}: {error}", None, None, None, None) + return {} + + def handle_verification_failure(self, model, model_path, file_extension): + print(f"Verification failed for model {model}. Retrying from GitLab...") + model_url = f"{GITLAB_URL}/Models/{model}.{file_extension}" + download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, model_url, MODEL_DOWNLOAD_PARAM, params_memory) + + if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): + handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + return + + if verify_download(model_path, model_url): + print(f"Model {model} downloaded and verified successfully!") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(MODEL_DOWNLOAD_PARAM) + self.downloading_model = False + else: + handle_error(model_path, "Verification failed...", "GitLab verification failed", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + + def download_model(self, model_to_download): + self.downloading_model = True + + repo_url = get_repository_url() + if not repo_url: + handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + return + + try: + model_index = self.available_models.index(model_to_download) + model_version = self.model_versions[model_index] + except Exception: + handle_error(None, f"Unknown model version for {model_to_download}! Download aborted.", "Model download failed", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + return + + if model_version in ("v8", "v9", "v10", "v11"): + # Download all PKL and metadata files for multi-file tinygrad models (v8 and v9) + filenames = [ + f"{model_to_download}_driving_policy_tinygrad.pkl", + f"{model_to_download}_driving_vision_tinygrad.pkl", + f"{model_to_download}_driving_policy_metadata.pkl", + f"{model_to_download}_driving_vision_metadata.pkl", + ] + for filename in filenames: + model_path = MODELS_PATH / filename + model_url = f"{repo_url}/Models/{filename}" + print(f"Downloading model file: {filename}") + download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, model_url, MODEL_DOWNLOAD_PARAM, params_memory) + + if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): + handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + return + + if verify_download(model_path, model_url): + print(f"File {filename} downloaded and verified successfully!") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloaded {filename}!") + else: + self.handle_verification_failure(filename[:-4], model_path, "pkl") + self.downloading_model = False + return + # After all files are downloaded and verified + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(MODEL_DOWNLOAD_PARAM) + + elif model_version == "v7": + # Download both PKL and metadata for OG tinygrad models + v7_filenames = [ + f"{model_to_download}.pkl", + f"{model_to_download}_metadata.pkl" + ] + for filename in v7_filenames: + model_path = MODELS_PATH / filename + model_url = f"{repo_url}/Models/{filename}" + print(f"Downloading v7 model file: {filename}") + download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, model_url, MODEL_DOWNLOAD_PARAM, params_memory) + + if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): + handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + return + + if verify_download(model_path, model_url): + print(f"File {filename} downloaded and verified successfully!") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloaded {filename}!") + else: + self.handle_verification_failure(filename.rsplit('.',1)[0], model_path, "pkl") + self.downloading_model = False + return + + # Once both files are fetched + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(MODEL_DOWNLOAD_PARAM) + + else: + # Classic model: download only the .thneed file + file_extension = "thneed" + model_path = MODELS_PATH / f"{model_to_download}.{file_extension}" + model_url = f"{repo_url}/Models/{model_to_download}.{file_extension}" + print(f"Downloading classic model: {model_to_download}") + download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, model_url, MODEL_DOWNLOAD_PARAM, params_memory) + + if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): + handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) + self.downloading_model = False + return + + if verify_download(model_path, model_url): + print(f"Model {model_to_download} downloaded and verified successfully!") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(MODEL_DOWNLOAD_PARAM) + else: + self.handle_verification_failure(model_to_download, model_path, file_extension) + self.downloading_model = False + return + + self.downloading_model = False + + @staticmethod + def copy_default_model(): + default_model_path = MODELS_PATH / f"{DEFAULT_MODEL}.thneed" + source_path = Path(__file__).parents[2] / "selfdrive/modeld/models/supercombo.thneed" + if source_path.is_file() and not default_model_path.is_file(): + shutil.copyfile(source_path, default_model_path) + print(f"Copied the default model from {source_path} to {default_model_path}") def check_models(self, boot_run, repo_url): - downloaded_models = [ - model for model in MODELS_PATH.iterdir() - if (MODELS_PATH / f"{model}.thneed").is_file() or all((MODELS_PATH / f"{model}_{filename}").is_file() for filename, _ in TINYGRAD_FILES) - ] - for model_file in downloaded_models: - if not any(model in model_file.name for model in set(self.available_models)): + available_models = set(self.available_models) - {DEFAULT_MODEL} + downloaded_models = set() + for model in available_models: + try: + model_index = self.available_models.index(model) + model_version = self.model_versions[model_index] + except Exception: + model_version = None + + if model_version in ("v8", "v9", "v10", "v11"): + v8_v9_files = [ + f"{model}_driving_policy_tinygrad.pkl", + f"{model}_driving_vision_tinygrad.pkl", + f"{model}_driving_policy_metadata.pkl", + f"{model}_driving_vision_metadata.pkl", + ] + if all((MODELS_PATH / f).is_file() for f in v8_v9_files): + downloaded_models.add(model) + elif model_version == "v7": + filename = f"{model}.pkl" + if (MODELS_PATH / filename).is_file(): + downloaded_models.add(model) + else: + filename = f"{model}.thneed" + if (MODELS_PATH / filename).is_file(): + downloaded_models.add(model) + + outdated_models = downloaded_models - available_models + for model in outdated_models: + for model_file in MODELS_PATH.glob(f"{model}*"): print(f"Removing outdated model: {model_file}") delete_file(model_file) @@ -65,466 +236,172 @@ class ModelManager: if tmp_file.is_file(): delete_file(tmp_file) - if params.get("Model", encoding="utf-8").removesuffix("_default") not in self.available_models: + if params.get("Model", encoding="utf-8") not in self.available_models: params.put("Model", params_default.get("Model", encoding="utf-8")) - if not (not boot_run and params.get_bool("AutomaticallyDownloadModels")): + automatically_download_models = params.get_bool("AutomaticallyDownloadModels") + if not automatically_download_models: return model_sizes = self.fetch_all_model_sizes(repo_url) if not model_sizes: - print("No model size data available. Skipping model checks...") - return + print("No model size data available. Continuing downloads based on file existence") + # do not return; proceed to download missing files - need_to_update_models = False - for model in self.available_models: - if self.is_tinygrad_model(model): - model_file = MODELS_PATH / f"{model}.thneed" - if not model_file.is_file(): - need_to_update_models = True - continue + needs_download = False - expected_size = model_sizes.get(model_file.name) - local_size = self.model_sizes.get(model_file.name) + # Enhanced model file validation per model version + for model in available_models: + model_version = None + try: + model_index = self.available_models.index(model) + model_version = self.model_versions[model_index] + except Exception: + model_version = None - if expected_size > 0 and local_size != expected_size: - print(f"Model {model} is outdated. Deleting {model_file}...") - delete_file(model_file) - need_to_update_models = True - else: - model_missing = False - model_outdated = False - - for filename, _ in TINYGRAD_FILES: - expected_file = MODELS_PATH / f"{model}_{filename}" - if not expected_file.is_file(): - model_missing = True - need_to_update_models = True + if model_version in ("v8", "v9", "v10", "v11"): + v8_v9_files = [ + f"{model}_driving_policy_tinygrad.pkl", + f"{model}_driving_vision_tinygrad.pkl", + f"{model}_driving_policy_metadata.pkl", + f"{model}_driving_vision_metadata.pkl", + ] + for filename in v8_v9_files: + path = MODELS_PATH / filename + expected_size = model_sizes.get(filename.rsplit(".", 1)[0]) + if not path.is_file() or expected_size is None or path.stat().st_size != expected_size: + needs_download = True break - - for filename, _ in TINYGRAD_FILES: - model_file = f"{model}_{filename}" - - expected_size = model_sizes.get(model_file) - local_size = self.model_sizes.get(model_file) - - if expected_size > 0 and local_size != expected_size: - model_outdated = True - need_to_update_models = True - break - - if model_missing or model_outdated: - print(f"Model {model} is either missing required files or outdated. Deleting...") - for filename, _ in TINYGRAD_FILES: - delete_file(MODELS_PATH / f"{model}_{filename}") - - if need_to_update_models: - params_memory.put_bool(MODEL_DOWNLOAD_ALL_PARAM, True) - - def check_tinygrad(self, repo_url): - tinygrad_url = f"{repo_url}/Tinygrad/{TAR_FILE_NAME}" - - expected_size = get_remote_file_size(tinygrad_url, self.session) - local_size = int(self.tinygrad_sizes.get(TAR_FILE_NAME, 0)) - - if expected_size > 0 and local_size != expected_size: - print(f"Tinygrad version {VERSION} is outdated, expected_size: {expected_size}, local_size: {local_size}, flagging for update...") - params.put_bool("TinygradUpdateAvailable", True) - - def copy_default_model(self): - classic_default_model_path = MODELS_PATH / "wd-40.thneed" - source_path = Path(__file__).parents[1] / "classic_modeld/models/supercombo.thneed" - if source_path.is_file() and (not classic_default_model_path.is_file() or source_path.stat().st_size != classic_default_model_path.stat().st_size): - shutil.copyfile(source_path, classic_default_model_path) - print(f"Copied the classic default model from {source_path} to {classic_default_model_path}") - self.update_model_size(classic_default_model_path) - - default_model_path = MODELS_PATH / "national-public-radio.thneed" - source_path = Path(__file__).parents[2] / "selfdrive/modeld/models/supercombo.thneed" - if source_path.is_file() and (not default_model_path.is_file() or source_path.stat().st_size != default_model_path.stat().st_size): - shutil.copyfile(source_path, default_model_path) - print(f"Copied the default model from {source_path} to {default_model_path}") - self.update_model_size(default_model_path) - - for filename, description in TINYGRAD_FILES: - source = TINYGRAD_MODELD_PATH / "models" / filename - target = MODELS_PATH / f"{DEFAULT_MODEL}_{filename}" - if source.is_file() and (not target.is_file() or source.stat().st_size != target.stat().st_size): - shutil.copyfile(source, target) - print(f"Copied the tinygrad {description} from {source} to {target}") - - def download_all_models(self): - repo_url = get_repository_url(self.session) - if not repo_url: - handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - return - - self.fetch_models(f"{repo_url}/Versions/model_names_{VERSION}.json", repo_url) - - for model in self.available_models: - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_ALL_PARAM, DOWNLOAD_PROGRESS_PARAM) - return - - if self.is_tinygrad_model(model): - already_downloaded = (MODELS_PATH / f"{model}.thneed").is_file() + elif model_version == "v7": + filename = f"{model}.pkl" + path = MODELS_PATH / filename + expected_size = model_sizes.get(model) + if not path.is_file() or expected_size is None or path.stat().st_size != expected_size: + needs_download = True else: - already_downloaded = all((MODELS_PATH / f"{model}_{filename}").is_file() for filename, _ in TINYGRAD_FILES) + filename = f"{model}.thneed" + path = MODELS_PATH / filename + expected_size = model_sizes.get(model) + if not path.is_file() or expected_size is None or path.stat().st_size != expected_size: + needs_download = True - if already_downloaded: - continue + if needs_download: + self.download_all_models() - print(f"Model {model} is not downloaded. Preparing to download...") - params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloading \"{self.available_model_names[self.available_models.index(model)]}\"...") - self.download_model(model) + def update_model_params(self, model_info, repo_url): + self.available_models = [model["id"] for model in model_info] + self.model_versions = [model["version"] for model in model_info] + self.model_series = [model.get("series", "Dom Forgot To Label Me") for model in model_info] - params_memory.put(DOWNLOAD_PROGRESS_PARAM, "All models downloaded!") - params_memory.remove(MODEL_DOWNLOAD_ALL_PARAM) + params.put("AvailableModels", ",".join(self.available_models)) + params.put("AvailableModelNames", ",".join([model["name"] for model in model_info])) + params.put("AvailableModelSeries", ",".join(self.model_series)) + params.put("ModelVersions", ",".join(self.model_versions)) + params.put("AvailableModelSeries", ",".join(self.model_series)) + print("Models list updated successfully") - def download_model(self, model_to_download): - self.downloading_model = True + # --- Generate per-model version JSON for offline UI --- + try: + versions_file = MODELS_PATH / ".model_versions.json" + version_map = {model_id: version for model_id, version in zip(self.available_models, self.model_versions)} + with open(versions_file, "w") as vf: + json.dump(version_map, vf) + except Exception as e: + print(f"Failed to write .model_versions.json: {e}") + # --- end JSON generation --- - repo_url = get_repository_url(self.session) - if not repo_url: - handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return + # Immediately sync the active ModelVersion param + try: + current = params.get("Model", encoding="utf-8") + if current in version_map: + params.put("ModelVersion", version_map[current]) + print(f"Successfully synced ModelVersion to {version_map[current]} for model {current}") + else: + print(f"Warning: Model {current} not found in version map") + except Exception as e: + print(f"Failed to sync ModelVersion for {current}: {e}") - if self.is_tinygrad_model(model_to_download): - model_path = MODELS_PATH / f"{model_to_download}.thneed" - model_url = f"{repo_url}/Models/{model_to_download}.thneed" + # Also ensure ModelVersion is set for the default model if not already set + try: + if not params.get("ModelVersion", encoding="utf-8"): + default_model = params.get("Model", encoding="utf-8") or DEFAULT_MODEL + if default_model in version_map: + params.put("ModelVersion", version_map[default_model]) + print(f"Set default ModelVersion to {version_map[default_model]} for model {default_model}") + except Exception as e: + print(f"Failed to set default ModelVersion: {e}") - print(f"Downloading model: {model_to_download}") - download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, model_url, MODEL_DOWNLOAD_PARAM, self.session) - - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - delete_file(model_path) - - handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return - - if verify_download(model_path, model_url, self.session): - print(f"Model {model_to_download} downloaded and verified successfully!") - self.update_model_size(model_path) - - params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - params_memory.remove(MODEL_DOWNLOAD_PARAM) - - self.downloading_model = False - return - - print(f"Verification failed for model {model_to_download}. Retrying from GitLab...") - fallback_url = f"{GITLAB_URL}/Models/{model_to_download}.thneed" - download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, fallback_url, MODEL_DOWNLOAD_PARAM, self.session) - - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - delete_file(model_path) - - handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return - - if verify_download(model_path, fallback_url, self.session): - print(f"Model {model_to_download} downloaded and verified successfully from GitLab!") - self.update_model_size(model_path) - - params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - params_memory.remove(MODEL_DOWNLOAD_PARAM) - - self.downloading_model = False - else: - handle_error(model_path, "Verification failed...", "GitLab verification failed", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - else: - all_model_sizes = self.fetch_all_model_sizes(repo_url) or {} - tinygrad_filenames = [f"{model_to_download}_{file_key}" for file_key, _ in TINYGRAD_FILES] - - file_sizes = [] - file_sources = [] - - missing = [name for name in tinygrad_filenames if int(all_model_sizes.get(name, 0)) <= 0] - if missing: - handle_error(None, "Missing size metadata...", f"Sizes not found for: {', '.join(missing)}...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return - - for filename in tinygrad_filenames: - primary_url = f"{repo_url}/Models/compiled/{filename}" - file_size = int(all_model_sizes.get(filename, 0)) - file_sizes.append(file_size) - file_sources.append((primary_url, None)) - - downloaded_offset_bytes = 0 - known_file_sizes = [size for size in file_sizes if size > 0] - total_model_bytes = sum(known_file_sizes) if len(known_file_sizes) == len(file_sizes) else 0 - - for (file_key, description), part_bytes, (primary_url, fallback_url) in zip(TINYGRAD_FILES, file_sizes, file_sources): - filename = f"{model_to_download}_{file_key}" - model_path = MODELS_PATH / filename - - print(f"Downloading {description} for model: {model_to_download}") - download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, primary_url, MODEL_DOWNLOAD_PARAM, self.session, offset_bytes=downloaded_offset_bytes, total_bytes=total_model_bytes) - - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - delete_file(model_path) - - handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return - - if verify_download(model_path, primary_url, self.session): - print(f"{description.capitalize()} for {model_to_download} downloaded and verified successfully!") - if total_model_bytes: - downloaded_offset_bytes += part_bytes - continue - - print(f"Verification failed for {filename}. Retrying from GitLab...") - fallback_url = f"{GITLAB_URL}/Models/compiled/{filename}" - download_file(CANCEL_DOWNLOAD_PARAM, model_path, DOWNLOAD_PROGRESS_PARAM, fallback_url, MODEL_DOWNLOAD_PARAM, self.session, offset_bytes=downloaded_offset_bytes, total_bytes=total_model_bytes) - - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - delete_file(model_path) - - handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return - - if verify_download(model_path, fallback_url, self.session): - print(f"{description.capitalize()} for {model_to_download} downloaded and verified successfully from GitLab!") - if total_model_bytes: - downloaded_offset_bytes += part_bytes - else: - handle_error(model_path, "Verification failed...", f"GitLab verification failed for {filename}", MODEL_DOWNLOAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - self.downloading_model = False - return - - print(f"Updating model sizes for {model_to_download}...") - for filename, _ in TINYGRAD_FILES: - file_path = MODELS_PATH / f"{model_to_download}_{filename}" - self.update_model_size(file_path) - - params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - params_memory.remove(MODEL_DOWNLOAD_PARAM) - - self.downloading_model = False - - def fetch_all_model_sizes(self, repo_url): - is_github = "github" in repo_url - is_gitlab = "gitlab" in repo_url - repo_encoded = quote_plus(RESOURCES_REPO) - - model_sizes = {} - try: - def fetch_dir_sizes(api_url): - sizes = {} - print(f"Fetching model metadata: {api_url}") - response = self.session.get(api_url, timeout=10) - response.raise_for_status() - content = response.json() - - model_files = [file for file in content if "." in file["name"]] - - if is_github: - for file in model_files: - sizes[file["name"]] = file.get("size", 0) - else: - for file in model_files: - file_path = quote_plus(file["path"]) - metadata_url = f"https://gitlab.com/api/v4/projects/{repo_encoded}/repository/files/{file_path}/raw?ref=Models" - head_response = self.session.head(metadata_url, timeout=10) - if head_response.ok: - sizes[file["name"]] = int(head_response.headers.get("content-length", 0)) - return sizes - - if is_github: - top_api_url = f"https://api.github.com/repos/{RESOURCES_REPO}/contents?ref=Models" - version_api_url = f"https://api.github.com/repos/{RESOURCES_REPO}/contents/compiled?ref=Models" - elif is_gitlab: - top_api_url = f"https://gitlab.com/api/v4/projects/{repo_encoded}/repository/tree?ref=Models" - version_api_url = f"https://gitlab.com/api/v4/projects/{repo_encoded}/repository/tree?path=compiled&ref=Models" - else: - print(f"Unsupported repository URL: {repo_url}") - return model_sizes - - model_sizes.update(fetch_dir_sizes(top_api_url)) - model_sizes.update(fetch_dir_sizes(version_api_url)) - - return model_sizes - - except requests.exceptions.RequestException as e: - handle_request_error(f"Failed to fetch model sizes from {'GitHub' if is_github else 'GitLab'}: {e}", None, None, None) - return {} - - def fetch_models(self, url, repo_url, boot_run=False): - try: - response = self.session.get(url, timeout=10) - response.raise_for_status() - model_info = response.json().get("models", []) - - if model_info: - self.update_model_params(model_info) - self.check_models(boot_run, repo_url) - self.check_tinygrad(repo_url) - except Exception as exception: - handle_request_error(exception, None, None, None) - return [] - - def is_tinygrad_model(self, model): - return self.model_versions[self.available_models.index(model)] in {"v1", "v2", "v3", "v4", "v5", "v6"} - - def update_model_params(self, model_info): - self.available_models = [model["id"] for model in model_info] - self.available_model_names = [model["name"] for model in model_info] - self.model_versions = [model["version"] for model in model_info] - - params.put("AvailableModels", ",".join(self.available_models)) - params.put("AvailableModelNames", ",".join(self.available_model_names)) - params.put("ModelVersions", ",".join(self.model_versions)) - print("Models list updated successfully!") - - def update_models(self, boot_run): + def update_models(self, boot_run=False): if self.downloading_model: return - repo_url = get_repository_url(self.session) + repo_url = get_repository_url() if repo_url is None: print("GitHub and GitLab are offline...") return - self.fetch_models(f"{repo_url}/Versions/model_names_{VERSION}.json", repo_url, boot_run) + model_info = self.fetch_models(f"{repo_url}/Versions/model_names_{VERSION}.json") + if model_info: + self.update_model_params(model_info, repo_url) + self.check_models(boot_run, repo_url) - def update_model_size(self, file_path): - self.model_sizes[file_path.name] = file_path.stat().st_size - update_json_file(self.model_sizes_path, self.model_sizes) - print(f"Updated size for {file_path.name} in {self.model_sizes_path.name}") + # Ensure ModelVersion is set immediately after updating model params + if boot_run: + try: + current = params.get("Model", encoding="utf-8") + if current and current in [model["id"] for model in model_info]: + model_index = [model["id"] for model in model_info].index(current) + version = model_info[model_index]["version"] + params.put("ModelVersion", version) + print(f"Boot sync: Set ModelVersion to {version} for model {current}") + except Exception as e: + print(f"Boot sync failed: {e}") - def update_tinygrad_size(self, file_path): - self.tinygrad_sizes[TAR_FILE_NAME] = file_path.stat().st_size - update_json_file(self.tinygrad_sizes_path, self.tinygrad_sizes) - print(f"Updated size for {TAR_FILE_NAME} in {self.tinygrad_sizes_path.name}") - - def update_tinygrad(self): - repo_url = get_repository_url(self.session) + def download_all_models(self): + repo_url = get_repository_url() if not repo_url: - handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", None, None) + handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", MODEL_DOWNLOAD_ALL_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) return - primary_url = f"{repo_url}/Tinygrad/{TAR_FILE_NAME}" - fallback_url = f"https://gitlab.com/{RESOURCES_REPO}/-/raw/Tinygrad/{TAR_FILE_NAME}" + model_info = self.fetch_models(f"{repo_url}/Versions/model_names_{VERSION}.json") + if model_info: + available_models = [model["id"] for model in model_info] + available_model_names = [re.sub(r"[πŸ—ΊοΈπŸ‘€πŸ“‘]", "", model["name"]).strip() for model in model_info] + model_versions = [model["version"] for model in model_info] + model_series = [model.get("series", "Dom Forgot To Label Me") for model in model_info] - tinygrad_tar_path = Path("/data/tmp/tinygrad.tar.gz") - try: - print(f"Attempting to download tinygrad from {primary_url}...") - download_file(CANCEL_DOWNLOAD_PARAM, tinygrad_tar_path, DOWNLOAD_PROGRESS_PARAM, primary_url, UPDATE_TINYGRAD_PARAM, self.session) - - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - delete_file(tinygrad_tar_path) - - handle_error(None, "Tinygrad update cancelled...", "Tinygrad update cancelled...", UPDATE_TINYGRAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - params_memory.remove("CancelModelDownload") - return - - if not verify_download(tinygrad_tar_path, primary_url, self.session): - print(f"Verification failed for {primary_url}. Retrying from GitLab...") - download_file(CANCEL_DOWNLOAD_PARAM, tinygrad_tar_path, DOWNLOAD_PROGRESS_PARAM, fallback_url, UPDATE_TINYGRAD_PARAM, self.session) - - if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - delete_file(tinygrad_tar_path) - - handle_error(None, "Tinygrad update cancelled...", "Tinygrad update cancelled...", UPDATE_TINYGRAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - params_memory.remove("CancelModelDownload") - return - - if not verify_download(tinygrad_tar_path, fallback_url, self.session): - handle_error(tinygrad_tar_path, "Verification Failed", "Tinygrad verification failed", UPDATE_TINYGRAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - return - - print("Tinygrad downloaded successfully! Proceeding with installation...") - self.update_tinygrad_size(tinygrad_tar_path) - - params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Installing...") - - print("Deleting old tinygrad directories...") - delete_file(TINYGRAD_MODELD_PATH) - print(f"Removed {TINYGRAD_MODELD_PATH}") - delete_file(TINYGRAD_REPO_PATH) - print(f"Removed {TINYGRAD_REPO_PATH}") - - extract_tar(tinygrad_tar_path, Path(BASEDIR)) - - print("Tinygrad update completed successfully!") - - params.put_bool("TinygradUpdateAvailable", False) - - params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Updated!") - params_memory.remove(UPDATE_TINYGRAD_PARAM) - - self.update_tinygrad_models(repo_url) - except Exception as exception: - handle_error(tinygrad_tar_path, "Update Failed", f"An unexpected error occurred: {exception}", UPDATE_TINYGRAD_PARAM, DOWNLOAD_PROGRESS_PARAM) - - def update_tinygrad_models(self, repo_url=None): - print("Updating old Tinygrad models...") - - installed_tinygrad_models = set() - for filename, _ in TINYGRAD_FILES: - suffix = f"_{filename}" - for file_path in MODELS_PATH.glob(f"*{suffix}"): - model_name = file_path.name.rsplit(suffix, 1)[0] - if model_name in set(self.available_models): - installed_tinygrad_models.add(model_name) - delete_file(file_path) - - self.copy_default_model() - - update_frogpilot_toggles() - - if repo_url is None: - return - - current_model = params.get("Model", encoding="utf-8").removesuffix("_default") - - models_to_redownload = [current_model] - models_to_redownload += [model for model in sorted(installed_tinygrad_models) if model != current_model] - - if DEFAULT_MODEL in models_to_redownload: - models_to_redownload.remove(DEFAULT_MODEL) - - if models_to_redownload: - print(f"Redownloading the following models: {', '.join(models_to_redownload)}") - self.fetch_models(f"{repo_url}/Versions/model_names_{VERSION}.json", repo_url, boot_run=True) - - for model in models_to_redownload: + for model, model_name, model_version in zip(available_models, available_model_names, model_versions): if params_memory.get_bool(CANCEL_DOWNLOAD_PARAM): - handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_ALL_PARAM, DOWNLOAD_PROGRESS_PARAM) + handle_error(None, "Download cancelled...", "Download cancelled...", MODEL_DOWNLOAD_ALL_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) return - params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloading \"{self.available_model_names[self.available_models.index(model)]}\"...") - self.download_model(model) + if model_version in ("v8", "v9", "v10", "v11"): + required_files = [ + f"{model}_driving_policy_tinygrad.pkl", + f"{model}_driving_vision_tinygrad.pkl", + f"{model}_driving_policy_metadata.pkl", + f"{model}_driving_vision_metadata.pkl", + ] + missing = [f for f in required_files if not (MODELS_PATH / f).is_file()] + if missing: + print(f"Tinygrad model {model} is missing files. Preparing to download...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloading \"{model_name}\"...") + self.download_model(model) + elif model_version == "v7": + # OG tinygrad: only need PKL file + model_file = MODELS_PATH / f"{model}.pkl" + if not model_file.is_file(): + print(f"PKL model {model} is missing. Preparing to download...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloading \"{model_name}\"...") + self.download_model(model) + else: + # Classic: only need .thneed + model_file = MODELS_PATH / f"{model}.thneed" + if not model_file.is_file(): + print(f"Classic model {model} is missing. Preparing to download...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, f"Downloading \"{model_name}\"...") + self.download_model(model) + + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "All models downloaded!") else: - print("No previously installed tinygrad models to redownload") - - update_frogpilot_toggles() - - def validate_models(self): - current = params.get("Model", encoding="utf-8") - default = params_default.get("Model", encoding="utf-8") - - if current.endswith("_default") and current != default: - print(f"Model '{current}' does not match default '{default}', resetting...") - params.put("Model", default) - - if VERSION_PATH.is_file(): - version_name = VERSION_PATH.read_text().strip() - if version_name != VERSION or int(self.tinygrad_sizes.get(TAR_FILE_NAME, 0)) == 0: - self.update_tinygrad_models() - - self.tinygrad_sizes[TAR_FILE_NAME] = DEFAULT_TINYGRAD_SIZE - update_json_file(self.tinygrad_sizes_path, self.tinygrad_sizes) - print(f"Updated size for {TAR_FILE_NAME} in {self.tinygrad_sizes_path.name}") - - params.remove("TinygradUpdateAvailable") - - VERSION_PATH.write_text(VERSION) - print(f"Updated {VERSION_PATH} to {VERSION}") + handle_error(None, "Unable to fetch models...", "Model list unavailable", MODEL_DOWNLOAD_ALL_PARAM, DOWNLOAD_PROGRESS_PARAM, params_memory) diff --git a/frogpilot/assets/other_images/frogpilot_boot_logo.png b/frogpilot/assets/other_images/frogpilot_boot_logo.png index bf6ddfd20..f502d99f4 100644 Binary files a/frogpilot/assets/other_images/frogpilot_boot_logo.png and b/frogpilot/assets/other_images/frogpilot_boot_logo.png differ diff --git a/frogpilot/assets/theme_manager.py b/frogpilot/assets/theme_manager.py index da372528c..edce598a0 100644 --- a/frogpilot/assets/theme_manager.py +++ b/frogpilot/assets/theme_manager.py @@ -96,7 +96,7 @@ class ThemeManager: def download_theme(self, theme_component, theme_name, asset_param, frogpilot_toggles): self.downloading_theme = True - repo_url = get_repository_url(self.session) + repo_url = get_repository_url() if not repo_url: handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", asset_param, DOWNLOAD_PROGRESS_PARAM) self.downloading_theme = False @@ -252,7 +252,7 @@ class ThemeManager: except requests.exceptions.RequestException as error: print(f"Request failed: {error}") - handle_request_error(f"Failed to fetch theme sizes from {'GitHub' if is_github else 'GitLab'}: {error}", None, None, None) + handle_request_error(f"Failed to fetch theme sizes from {'GitHub' if is_github else 'GitLab'}: {error}", None, None, None, None) return {} @staticmethod @@ -563,7 +563,7 @@ class ThemeManager: if self.downloading_theme: return - repo_url = get_repository_url(self.session) + repo_url = get_repository_url() if repo_url is None: print("GitHub and GitLab are offline...") return diff --git a/frogpilot/common/frogpilot_functions.py b/frogpilot/common/frogpilot_functions.py index 1dbf5a242..ec8cb8dce 100644 --- a/frogpilot/common/frogpilot_functions.py +++ b/frogpilot/common/frogpilot_functions.py @@ -150,7 +150,7 @@ def frogpilot_boot_functions(build_metadata, params_cache): params_cache.clear_all() FrogPilotVariables().update(holiday_theme="stock", started=False) - ModelManager(boot_run=True) + ModelManager() ThemeManager(boot_run=True).update_active_theme(time_validated=system_time_valid(), frogpilot_toggles=get_frogpilot_toggles(), boot_run=True) if VIDEO_CACHE_PATH.exists(): diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index c84094b41..f20eb304f 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -29,6 +29,8 @@ params = Params() params_cache = Params("/cache/params") params_default = Params("/dev/shm/params_default") params_memory = Params("/dev/shm/params") +params_tracking = Params("/cache/tracking") +params_tracking = Params("/cache/tracking") GearShifter = car.CarState.GearShifter SafetyModel = car.CarParams.SafetyModel @@ -40,11 +42,16 @@ EARTH_RADIUS = 6378137 # Radius of the Earth in meters MAX_T_FOLLOW = 3.0 # Maximum allowed following duration. Larger values risk losing track of the lead but may be increased as models improve MINIMUM_LATERAL_ACCELERATION = 1.3 # m/s^2, typical minimum lateral acceleration when taking curves PLANNER_TIME = ModelConstants.T_IDXS[-1] # Length of time the model projects out for + THRESHOLD = 0.63 # Requires the condition to be true for ~1 second +def scale_threshold(v_ego):#0 40 60 80 100 0 40 60 80 100 + # More aggressive with hysteresis and lead probability: faster activation at higher speeds + return np.interp(v_ego, [0, 17.9, 26.8, 35.8, 44.7], [0.58, 0.60, 0.62, 0.75, 0.9]) + NON_DRIVING_GEARS = [GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown] -RESOURCES_REPO = "FrogAi/FrogPilot-Resources" +RESOURCES_REPO = "firestar5683/StarPilot-Resources" ACTIVE_THEME_PATH = Path(__file__).parents[1] / "assets/active_theme" METADATAS_PATH = Path(__file__).parents[1] / "assets/model_metadata" @@ -66,12 +73,11 @@ KONIK_PATH = Path("/cache/use_konik") MAPD_PATH = Path("/data/media/0/osm/mapd") MAPS_PATH = Path("/data/media/0/osm/offline") - NNFF_MODELS_PATH = Path(BASEDIR) / "frogpilot/assets/nnff_models" DEFAULT_MODEL = "firehose" DEFAULT_MODEL_NAME = "Firehose (Default) πŸ‘€πŸ“‘" -DEFAULT_MODEL_VERSION = "v9" +DEFAULT_MODEL_VERSION = "v11" BUTTON_FUNCTIONS = { "NOTHING": 0, @@ -96,6 +102,7 @@ TINYGRAD_FILES = [ ("driving_vision_tinygrad.pkl", "vision model"), ] + @cache def get_nnff_model_files(): model_dir = Path(NNFF_MODELS_PATH) @@ -117,11 +124,11 @@ def update_frogpilot_toggles(): frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("AccelerationPath", "1", 2, "0"), ("AccelerationProfile", "2", 0, "0"), - ("AdjacentLeadsUI", "1", 3, "0"), + ("AdjacentLeadsUI", "0", 3, "0"), ("AdjacentPath", "0", 3, "0"), ("AdjacentPathMetrics", "0", 3, "0"), ("AdvancedCustomUI", "0", 2, "0"), - ("AdvancedLateralTune", "0", 3, "0"), + ("AdvancedLateralTune", "0", 2, "0"), ("AdvancedLongitudinalTune", "0", 3, "0"), ("AggressiveFollow", "1.25", 2, "1.25"), ("AggressiveJerkAcceleration", "50", 3, "50"), @@ -133,13 +140,14 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("AlertVolumeControl", "0", 2, "0"), ("AlwaysOnDM", "0", 0, "0"), ("AlwaysOnLateral", "1", 0, "0"), - ("AlwaysOnLateralLKAS", "1", 2, "0"), - ("AlwaysOnLateralMain", "1", 2, "0"), + ("AlwaysOnLateralLKAS", "1", 0, "0"), + ("AlwaysOnLateralMain", "1", 0, "0"), ("AMapKey1", "", 0, ""), ("AMapKey2", "", 0, ""), ("AutomaticallyDownloadModels", "1", 1, "0"), ("AutomaticUpdates", "1", 0, "1"), ("AvailableModelNames", "", 1, ""), + ("AvailableModelSeries", "", 1, ""), ("AvailableModels", "", 1, ""), ("BigMap", "0", 2, "0"), ("BlacklistedModels", "", 2, ""), @@ -158,7 +166,7 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("CELead", "0", 1, "0"), ("CEModelStopTime", str(PLANNER_TIME - 2), 2, "0"), ("CENavigation", "1", 2, "0"), - ("CENavigationIntersections", "0", 2, "0"), + ("CENavigationIntersections", "1", 2, "0"), ("CENavigationLead", "1", 2, "0"), ("CENavigationTurns", "1", 2, "0"), ("CESignalSpeed", "55", 2, "0"), @@ -203,17 +211,17 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("DistanceButtonControl", "1", 2, "0"), ("DriverCamera", "0", 1, "0"), ("DynamicPathWidth", "0", 2, "0"), - ("DynamicPedalsOnUI", "1", 1, "0"), + ("DynamicPedalsOnUI", "1", 2, "0"), ("EngageVolume", "101", 2, "101"), ("ExperimentalGMTune", "0", 2, "0"), ("ExperimentalLongitudinalEnabled", "0", 0, "0"), ("ExperimentalModeConfirmed", "0", 0, "0"), ("Fahrenheit", "0", 3, "0"), ("FavoriteDestinations", "", 0, ""), - ("ForceAutoTune", "0", 3, "0"), - ("ForceAutoTuneOff", "0", 3, "0"), + ("ForceAutoTune", "0", 2, "0"), + ("ForceAutoTuneOff", "0", 2, "0"), ("ForceFingerprint", "0", 2, "0"), - ("ForceMPHDashboard", "0", 3, "0"), + ("ForceMPHDashboard", "0", 2, "0"), ("ForceStops", "0", 2, "0"), ("ForceTorqueController", "0", 3, "0"), ("FPSCounter", "1", 3, "0"), @@ -235,10 +243,10 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("HideMaxSpeed", "0", 2, "0"), ("HideSpeed", "0", 2, "0"), ("HideSpeedLimit", "0", 2, "0"), - ("HigherBitrate", "0", 2, "0"), + ("HigherBitrate", "0", 3, "0"), ("HolidayThemes", "1", 0, "0"), - ("HumanAcceleration", "1", 2, "0"), - ("HumanFollowing", "1", 2, "0"), + ("HumanAcceleration", "0", 2, "0"), + ("HumanFollowing", "0", 2, "0"), ("IncreasedStoppedDistance", "0", 1, "0"), ("IncreaseThermalLimits", "0", 2, "0"), ("IsLdwEnabled", "0", 0, "0"), @@ -246,13 +254,13 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("KonikDongleId", "", 0, ""), ("KonikMinutes", "0", 0, "0"), ("LaneChanges", "1", 0, "1"), - ("LaneChangeTime", "1.0", 1, "0"), - ("LaneDetectionWidth", "0", 1, "0"), + ("LaneChangeTime", "2.0", 0, "0"), + ("LaneDetectionWidth", "0", 2, "0"), ("LaneLinesWidth", "4", 2, "2"), - ("LateralTune", "1", 1, "0"), + ("LateralTune", "1", 2, "0"), ("LeadDepartingAlert", "0", 0, "0"), ("LeadDetectionThreshold", "35", 3, "50"), - ("LeadInfo", "1", 3, "0"), + ("LeadInfo", "1", 2, "0"), ("LiveDelay", "", 0, ""), ("LKASButtonControl", "5", 2, "0"), ("LockDoors", "1", 0, "0"), @@ -263,17 +271,17 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("LongitudinalTune", "1", 0, "0"), ("LongPitch", "1", 2, "0"), ("LoudBlindspotAlert", "0", 0, "0"), - ("LowVoltageShutdown", str(VBATT_PAUSE_CHARGING), 3, str(VBATT_PAUSE_CHARGING)), + ("LowVoltageShutdown", str(VBATT_PAUSE_CHARGING), 2, str(VBATT_PAUSE_CHARGING)), ("MapAcceleration", "0", 1, "0"), ("MapboxPublicKey", "", 0, ""), ("MapboxSecretKey", "", 0, ""), ("MapDeceleration", "0", 1, "0"), - ("MapGears", "0", 2, "0"), + ("MapGears", "0", 1, "0"), ("MapsSelected", "", 0, ""), ("MapStyle", "1", 2, "0"), ("MaxDesiredAcceleration", "4.0", 2, "2.0"), ("MinimumLaneChangeSpeed", str(LANE_CHANGE_SPEED_MIN / CV.MPH_TO_MS), 2, str(LANE_CHANGE_SPEED_MIN / CV.MPH_TO_MS)), - ("Model", DEFAULT_MODEL + "_default", 1, DEFAULT_MODEL + "_default"), + ("Model", DEFAULT_MODEL, 1, DEFAULT_MODEL), ("ModelDrivesAndScores", "", 2, ""), ("ModelRandomizer", "0", 2, "0"), ("ModelUI", "1", 2, "0"), @@ -281,13 +289,13 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("NavigationUI", "1", 1, "0"), ("NavSettingLeftSide", "0", 0, "0"), ("NavSettingTime24h", "0", 0, "0"), - ("NewLongAPI", "1", 3, "1"), + ("NewLongAPI", "0", 2, "1"), ("NNFF", "1", 2, "0"), ("NNFFLite", "1", 2, "0"), ("NoLogging", "0", 2, "0"), ("NoUploads", "0", 2, "0"), - ("NudgelessLaneChange", "1", 0, "0"), - ("NumericalTemp", "1", 3, "0"), + ("NudgelessLaneChange", "0", 0, "0"), + ("NumericalTemp", "1", 2, "0"), ("Offset1", "5", 0, "0"), ("Offset2", "5", 0, "0"), ("Offset3", "5", 0, "0"), @@ -300,15 +308,15 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("openpilotMinutes", "0", 0, "0"), ("PathEdgeWidth", "20", 2, "0"), ("PathWidth", "6.1", 2, "5.9"), - ("PauseAOLOnBrake", "0", 1, "0"), - ("PauseLateralOnSignal", "0", 1, "0"), - ("PauseLateralSpeed", "0", 1, "0"), - ("PedalsOnUI", "0", 1, "0"), + ("PauseAOLOnBrake", "0", 2, "0"), + ("PauseLateralOnSignal", "0", 2, "0"), + ("PauseLateralSpeed", "0", 2, "0"), + ("PedalsOnUI", "0", 2, "0"), ("PersonalizeOpenpilot", "1", 0, "0"), ("PreferredSchedule", "2", 0, "0"), ("PromptDistractedVolume", "101", 2, "101"), ("PromptVolume", "101", 2, "101"), - ("QOLLateral", "1", 1, "0"), + ("QOLLateral", "1", 2, "0"), ("QOLLongitudinal", "1", 1, "0"), ("QOLVisuals", "1", 0, "0"), ("RadarTracksUI", "0", 3, "0"), @@ -318,19 +326,19 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("RecordFront", "0", 0, "0"), ("RefuseVolume", "101", 2, "101"), ("RelaxedFollow", "1.75", 2, "1.75"), - ("RelaxedJerkAcceleration", "100", 3, "100"), + ("RelaxedJerkAcceleration", "50", 3, "50"), ("RelaxedJerkDanger", "100", 3, "100"), - ("RelaxedJerkDeceleration", "100", 3, "100"), - ("RelaxedJerkSpeed", "100", 3, "100"), - ("RelaxedJerkSpeedDecrease", "100", 3, "100"), + ("RelaxedJerkDeceleration", "50", 3, "50"), + ("RelaxedJerkSpeed", "50", 3, "50"), + ("RelaxedJerkSpeedDecrease", "50", 3, "50"), ("RelaxedPersonalityProfile", "1", 2, "0"), ("ReverseCruise", "0", 1, "0"), ("RoadEdgesWidth", "2", 2, "2"), - ("RoadNameUI", "1", 1, "0"), + ("RoadNameUI", "1", 2, "0"), ("RotatingWheel", "1", 1, "0"), ("ScreenBrightness", "101", 2, "101"), ("ScreenBrightnessOnroad", "101", 2, "101"), - ("ScreenManagement", "1", 1, "0"), + ("ScreenManagement", "1", 2, "0"), ("ScreenRecorder", "1", 2, "0"), ("ScreenTimeout", "30", 2, "30"), ("ScreenTimeoutOnroad", "30", 2, "10"), @@ -349,12 +357,12 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("ShowSLCOffset", "1", 0, "0"), ("ShowSpeedLimits", "1", 1, "0"), ("ShowSteering", "0", 3, "0"), - ("ShowStoppingPoint", "1", 3, "0"), - ("ShowStoppingPointMetrics", "1", 3, "0"), + ("ShowStoppingPoint", "0", 2, "0"), + ("ShowStoppingPointMetrics", "0", 2, "0"), ("ShowStorageLeft", "0", 3, "0"), ("ShowStorageUsed", "0", 3, "0"), ("Sidebar", "0", 0, "0"), - ("SignalMetrics", "0", 3, "0"), + ("SignalMetrics", "0", 2, "0"), ("SLCConfirmation", "0", 0, "0"), ("SLCConfirmationHigher", "0", 0, "0"), ("SLCConfirmationLower", "0", 0, "0"), @@ -367,24 +375,24 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("SLCPriority2", "Map Data", 2, "Map Data"), ("SLCPriority3", "Dashboard", 2, "Dashboard"), ("SNGHack", "1", 2, "0"), - ("SpeedLimitChangedAlert", "0", 0, "0"), + ("SpeedLimitChangedAlert", "1", 0, "0"), ("SpeedLimitController", "1", 0, "0"), ("SpeedLimitFiller", "0", 0, "0"), - ("SpeedLimitSources", "0", 3, "0"), + ("SpeedLimitSources", "0", 2, "0"), ("SshEnabled", "0", 0, "0"), ("StartupMessageBottom", "Human-tested, frog-approved 🐸", 0, "Always keep hands on wheel and eyes on road"), ("StartupMessageTop", "Hop in and buckle up!", 0, "Be ready to take over at any time"), ("StandardFollow", "1.45", 2, "1.45"), - ("StandardJerkAcceleration", "100", 3, "100"), + ("StandardJerkAcceleration", "50", 3, "50"), ("StandardJerkDanger", "100", 3, "100"), - ("StandardJerkDeceleration", "100", 3, "100"), - ("StandardJerkSpeed", "100", 3, "100"), - ("StandardJerkSpeedDecrease", "100", 3, "100"), + ("StandardJerkDeceleration", "50", 3, "50"), + ("StandardJerkSpeed", "50", 3, "50"), + ("StandardJerkSpeedDecrease", "50", 3, "50"), ("StandardPersonalityProfile", "1", 2, "0"), - ("StandbyMode", "0", 1, "0"), + ("StandbyMode", "0", 2, "0"), ("StartAccel", "", 3, ""), ("StartAccelStock", "", 3, ""), - ("StaticPedalsOnUI", "0", 1, "0"), + ("StaticPedalsOnUI", "0", 2, "0"), ("SteerDelay", "", 3, ""), ("SteerDelayStock", "", 3, ""), ("SteerFriction", "", 3, ""), @@ -403,8 +411,6 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("TacoTune", "0", 2, "0"), ("TacoTuneHacks", "0", 2, "0"), ("TetheringEnabled", "0", 0, "0"), - ("ThemesDownloaded", "", 0, ""), - ("TinygradUpdateAvailable", "0", 1, "0"), ("ToyotaDoors", "1", 0, "0"), ("TrafficFollow", "0.5", 2, "0.5"), ("TrafficJerkAcceleration", "50", 3, "50"), @@ -431,7 +437,8 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("WarningImmediateVolume", "101", 2, "101"), ("WarningSoftVolume", "101", 2, "101"), ("WheelIcon", "frog", 0, "stock"), - ("WheelSpeed", "0", 2, "0") + ("WheelSpeed", "0", 2, "0"), + ("StopDistance", "6", 3, "6") ] misc_tuning_levels: list[tuple[str, str | bytes, int, str]] = [ @@ -538,6 +545,7 @@ class FrogPilotVariables: if not is_torque_car: CarInterfaceBase.configure_torque_tune(MOCK.MOCK, FPCP.lateralTuning) + toggle.always_on_lateral_set = bool(CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) toggle.car_make = CP.carName toggle.car_model = CP.carFingerprint @@ -567,7 +575,6 @@ class FrogPilotVariables: toggle.use_lkas_for_aol = not toggle.openpilot_longitudinal and CP.safetyConfigs[0].safetyModel == SafetyModel.hyundaiCanfd toggle.vEgoStarting = CP.vEgoStarting toggle.vEgoStopping = CP.vEgoStopping - msg_bytes = params.get("LiveTorqueParameters") if msg_bytes: with log.LiveTorqueParametersData.from_bytes(msg_bytes) as LTP: @@ -609,6 +616,8 @@ class FrogPilotVariables: toggle.vEgoStarting = np.clip(params.get_float("VEgoStarting"), 0.01, 1) if advanced_longitudinal_tuning and tuning_level >= level["VEgoStarting"] else toggle.vEgoStarting toggle.vEgoStopping = np.clip(params.get_float("VEgoStopping"), 0.01, 1) if advanced_longitudinal_tuning and tuning_level >= level["VEgoStopping"] else toggle.vEgoStopping + toggle.stop_distance = params.get_float("StopDistance") if advanced_longitudinal_tuning and tuning_level >= level["StopDistance"] else 6.0 + toggle.alert_volume_controller = params.get_bool("AlertVolumeControl") if tuning_level >= level["AlertVolumeControl"] else default.get_bool("AlertVolumeControl") toggle.disengage_volume = params.get_int("DisengageVolume") if toggle.alert_volume_controller and tuning_level >= level["DisengageVolume"] else default.get_int("DisengageVolume") toggle.engage_volume = params.get_int("EngageVolume") if toggle.alert_volume_controller and tuning_level >= level["EngageVolume"] else default.get_int("EngageVolume") @@ -816,31 +825,43 @@ class FrogPilotVariables: toggle.max_desired_acceleration = np.clip(params.get_float("MaxDesiredAcceleration"), 0.1, 4.0) if longitudinal_tuning and tuning_level >= level["MaxDesiredAcceleration"] else default.get_float("MaxDesiredAcceleration") toggle.taco_tune = longitudinal_tuning and (params.get_bool("TacoTune") if tuning_level >= level["TacoTune"] else default.get_bool("TacoTune")) - toggle.available_models = (params.get("AvailableModels", encoding="utf-8") or "") + f",{DEFAULT_MODEL}" - toggle.available_model_names = (params.get("AvailableModelNames", encoding="utf-8") or "") + f",{DEFAULT_MODEL_NAME}" - downloaded_models = [model for model in toggle.available_models.split(",") if (MODELS_PATH / f"{model}.thneed").is_file() or all((MODELS_PATH / f"{model}_{filename}").is_file() for filename, _ in TINYGRAD_FILES)] - model_versions = (params.get("ModelVersions", encoding="utf-8") or "") + f",{DEFAULT_MODEL_VERSION}" - toggle.model_randomizer = params.get_bool("ModelRandomizer") if tuning_level >= level["ModelRandomizer"] else default.get_bool("ModelRandomizer") - if toggle.model_randomizer: - if not started: - blacklisted_models = (params.get("BlacklistedModels", encoding="utf-8") or "").split(",") - selectable_models = [model for model in downloaded_models if model not in blacklisted_models] - toggle.model = random.choice(selectable_models) if selectable_models else DEFAULT_MODEL - toggle.model_name = "Mystery Model πŸ‘»" - toggle.model_version = model_versions.split(",")[toggle.available_models.split(",").index(toggle.model)] - else: - model = ((params.get("Model", encoding="utf-8") if tuning_level >= level["Model"] else default.get("Model", encoding="utf-8")) or DEFAULT_MODEL).removesuffix("_default") - if model in downloaded_models: - toggle.model = model - toggle.model_name = dict(zip(toggle.available_models.split(","), toggle.available_model_names.split(",")))[toggle.model] - toggle.model_version = dict(zip(toggle.available_models.split(","), model_versions.split(",")))[toggle.model] + toggle.available_models = params.get("AvailableModels", encoding="utf-8") or "" + toggle.available_model_names = params.get("AvailableModelNames", encoding="utf-8") or "" + toggle.available_model_series = params.get("AvailableModelSeries", encoding="utf-8") or "" + toggle.model_versions = params.get("ModelVersions", encoding="utf-8") or "" + toggle.available_model_series = params.get("AvailableModelSeries", encoding="utf-8") or "" + downloaded_models = [model for model in toggle.available_models.split(",") if any(MODELS_PATH.glob(f"{model}*"))] + toggle.model_randomizer = downloaded_models and (params.get_bool("ModelRandomizer") if tuning_level >= level["ModelRandomizer"] else default.get_bool("ModelRandomizer")) + if toggle.available_models and toggle.available_model_names and downloaded_models and toggle.model_versions: + if DEFAULT_MODEL not in toggle.available_models.split(","): + toggle.available_models += f",{DEFAULT_MODEL}" + toggle.available_model_names += f",{DEFAULT_MODEL_NAME}" + toggle.model_versions += f",{DEFAULT_MODEL_VERSION}" + downloaded_models += [DEFAULT_MODEL] + if toggle.model_randomizer: + if not started: + blacklisted_models = (params.get("BlacklistedModels", encoding="utf-8") or "").split(",") + selectable_models = [model for model in downloaded_models if model not in blacklisted_models] + toggle.model = random.choice(selectable_models) if selectable_models else default.get("Model", encoding="utf-8") + toggle.model_name = "Mystery Model πŸ‘»" + toggle.model_version = toggle.model_versions.split(",")[toggle.available_models.split(",").index(toggle.model)] else: + toggle.model = params.get("Model", encoding="utf-8") if tuning_level >= level["Model"] else default.get("Model", encoding="utf-8") + if toggle.model in downloaded_models: + toggle.model_name = toggle.available_model_names.split(",")[toggle.available_models.split(",").index(toggle.model)] + toggle.model_version = toggle.model_versions.split(",")[toggle.available_models.split(",").index(toggle.model)] + else: + toggle.model = default.get("Model", encoding="utf-8") + toggle.model_name = toggle.available_model_names.split(",")[toggle.available_models.split(",").index(toggle.model)] + toggle.model_version = toggle.model_versions.split(",")[toggle.available_models.split(",").index(toggle.model)] + else: toggle.model = DEFAULT_MODEL toggle.model_name = DEFAULT_MODEL_NAME toggle.model_version = DEFAULT_MODEL_VERSION + toggle.classic_longitudinal = toggle.model_version in {"v1", "v2", "v3", "v4"} toggle.classic_model = toggle.model_version in {"v1", "v2", "v3", "v4"} - toggle.classic_longitudinal = toggle.model_version in {"v1", "v2", "v3", "v4", "v5", "v6"} - toggle.tinygrad_model = not toggle.classic_model and toggle.model_version not in {"v5", "v6"} + toggle.tinygrad_model = toggle.model_version in {"v8", "v9", "v10", "v11"} + toggle.tomb_raider = toggle.model == "space-lab" toggle.model_ui = params.get_bool("ModelUI") if tuning_level >= level["ModelUI"] else default.get_bool("ModelUI") toggle.dynamic_path_width = toggle.model_ui and (params.get_bool("DynamicPathWidth") if tuning_level >= level["DynamicPathWidth"] else default.get_bool("DynamicPathWidth")) @@ -906,7 +927,6 @@ class FrogPilotVariables: toggle.screen_timeout = params.get_int("ScreenTimeout") if screen_management and tuning_level >= level["ScreenTimeout"] else default.get_int("ScreenTimeout") toggle.screen_timeout_onroad = params.get_int("ScreenTimeoutOnroad") if screen_management and tuning_level >= level["ScreenTimeoutOnroad"] else default.get_int("ScreenTimeoutOnroad") toggle.standby_mode = screen_management and (params.get_bool("StandbyMode") if tuning_level >= level["StandbyMode"] else default.get_bool("StandbyMode")) - toggle.sng_hack = toggle.openpilot_longitudinal and toggle.car_make == "toyota" and not toggle.has_pedal and not has_sng and (params.get_bool("SNGHack") if tuning_level >= level["SNGHack"] else default.get_bool("SNGHack")) toggle.speed_limit_controller = toggle.openpilot_longitudinal and (params.get_bool("SpeedLimitController") if tuning_level >= level["SpeedLimitController"] else default.get_bool("SpeedLimitController")) diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 841b669a4..99e66778b 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -9,7 +9,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST from openpilot.frogpilot.common.frogpilot_utilities import calculate_lane_width, calculate_road_curvature from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD, params, params_memory @@ -115,7 +115,9 @@ class FrogPilotPlanner: def update_lead_status(self): following_lead = self.lead_one.status - following_lead &= self.lead_one.dRel < self.model_length + STOP_DISTANCE + from frogpilot.common.frogpilot_variables import get_frogpilot_toggles + fp_toggles = get_frogpilot_toggles() + following_lead &= self.lead_one.dRel < self.model_length + fp_toggles.stop_distance self.tracking_lead_filter.update(following_lead) return self.tracking_lead_filter.x >= THRESHOLD diff --git a/frogpilot/controls/lib/conditional_experimental_mode.py b/frogpilot/controls/lib/conditional_experimental_mode.py index be8111d52..6f8c05831 100644 --- a/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/frogpilot/controls/lib/conditional_experimental_mode.py @@ -1,20 +1,54 @@ #!/usr/bin/env python3 from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL +from openpilot.common.numpy_fast import interp +from openpilot.common.conversions import Conversions as CV -from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory +from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory, scale_threshold class ConditionalExperimentalMode: + # ===== CONDITIONAL EXPERIMENTAL MODE SPEED-BASED TUNING ===== + # Speed ranges: [0-35, 35-55, 55-70, 70+ mph] + + # FILTER TIME CONSTANTS (Lower = More responsive, Higher = Smoother) + # [City, Urban Hwy, Rural Hwy, High Speed] + FILTER_TIME_CURVES = [0.9, 0.8, 0.6, 0.5] # Faster detection at highway speeds + FILTER_TIME_LEADS = [0.9, 0.8, 0.7, 0.5] # Less sensitive at 70+ mph for slow leads + FILTER_TIME_LIGHTS = [0.9, 0.8, 0.75, 0.55] # Less sensitive at 60+ mph for stoplights + + # HIGHWAY LIGHT DETECTION MULTIPLIERS + # How much to increase model stop time at highway speeds + LIGHT_BOOSTS = [1.0, 1.2, 1.1, 1.0] # Keep conservative boost for highest speeds + LIGHT_SPEED_LOW = 50 * CV.MPH_TO_MS # 50 mph threshold + LIGHT_SPEED_HIGH = 60 * CV.MPH_TO_MS # 60 mph threshold + LIGHT_MAX_TIME = 9 # Balanced max time preserving city performance + + # ===== END TUNING PARAMETERS ===== + + # Current active values + FILTER_TIME_CURVE = 0.8 + FILTER_TIME_LEAD = 0.8 + FILTER_TIME_LIGHT = 0.8 + LIGHT_BOOST_LOW = 1.15 + LIGHT_BOOST_HIGH = 1.2 + + @staticmethod + def get_speed_based_param(speed_mph, param_array): + """Get parameter value based on current speed using smooth interpolation between breakpoints [0, 35, 55, 70]""" + return interp(speed_mph, [0, 35, 55, 70], param_array) + def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.curvature_filter = FirstOrderFilter(0, 1, DT_MDL) - self.slow_lead_filter = FirstOrderFilter(0, 1, DT_MDL) - self.stop_light_filter = FirstOrderFilter(0, 0.5, DT_MDL) + # Faster filters with hysteresis for better responsiveness + self.curvature_filter = FirstOrderFilter(0, self.FILTER_TIME_CURVE, DT_MDL) + self.slow_lead_filter = FirstOrderFilter(0, self.FILTER_TIME_LEAD, DT_MDL) + self.stop_light_filter = FirstOrderFilter(0, self.FILTER_TIME_LIGHT, DT_MDL) self.curve_detected = False self.experimental_mode = False self.stop_light_detected = False + self.prev_experimental_mode = False # For hysteresis def update(self, v_ego, sm, frogpilot_toggles): if frogpilot_toggles.experimental_mode_via_press: @@ -24,9 +58,26 @@ class ConditionalExperimentalMode: if self.status_value not in {1, 2} and not sm["carState"].standstill: self.update_conditions(v_ego, sm, frogpilot_toggles) + new_experimental_mode = self.check_conditions(v_ego, sm, frogpilot_toggles) + + # Add hysteresis to prevent rapid toggling + if new_experimental_mode and not self.prev_experimental_mode: + # Require weaker conditions to turn on + hysteresis_factor = 0.9 + elif not new_experimental_mode and self.prev_experimental_mode: + # Require stronger conditions to turn off + hysteresis_factor = 1.2 + else: + hysteresis_factor = 1.0 + + # Apply hysteresis to key conditions + if hasattr(self, 'slow_lead_detected'): + self.slow_lead_detected = self.slow_lead_detected if hysteresis_factor == 1.0 else (self.slow_lead_filter.x >= scale_threshold(v_ego) * hysteresis_factor) + if hasattr(self, 'curve_detected'): + self.curve_detected = self.curve_detected if hysteresis_factor == 1.0 else (self.curvature_filter.x >= THRESHOLD * hysteresis_factor) self.experimental_mode = self.check_conditions(v_ego, sm, frogpilot_toggles) - + self.prev_experimental_mode = self.experimental_mode params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) else: self.experimental_mode = self.status_value == 2 or sm["carState"].standstill and self.experimental_mode and self.frogpilot_planner.model_stopped @@ -55,7 +106,7 @@ class ConditionalExperimentalMode: self.status_value = 8 return True - if frogpilot_toggles.conditional_lead and self.slow_lead_detected: + if frogpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31: self.status_value = 9 if self.frogpilot_planner.lead_one.vLead < 1 else 10 return True @@ -71,30 +122,70 @@ class ConditionalExperimentalMode: def update_conditions(self, v_ego, sm, frogpilot_toggles): self.curve_detection(v_ego, frogpilot_toggles) - self.slow_lead(frogpilot_toggles) + self.slow_lead(frogpilot_toggles, v_ego) self.stop_sign_and_light(v_ego, sm, frogpilot_toggles.conditional_model_stop_time) def curve_detection(self, v_ego, frogpilot_toggles): self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or self.frogpilot_planner.driving_in_curve) self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED - def slow_lead(self, frogpilot_toggles): + def slow_lead(self, frogpilot_toggles, v_ego): if self.frogpilot_planner.tracking_lead: slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead stopped_lead = frogpilot_toggles.conditional_stopped_lead and self.frogpilot_planner.lead_one.vLead < 1 + lead_threshold = scale_threshold(v_ego) + + # Adjust threshold based on lead probability for vision-only accuracy + lead_prob = getattr(self.frogpilot_planner.lead_one, 'modelProb', 1.0) + adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence self.slow_lead_filter.update(slower_lead or stopped_lead) - self.slow_lead_detected = self.slow_lead_filter.x >= THRESHOLD + self.slow_lead_detected = self.slow_lead_filter.x >= adjusted_threshold else: self.slow_lead_filter.x = 0 self.slow_lead_detected = False def stop_sign_and_light(self, v_ego, sm, model_time): if not sm["frogpilotCarState"].trafficModeEnabled: - model_stopping = self.frogpilot_planner.model_length < v_ego * model_time + speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph + + # Interp for smooth scaling in 35-45 mph + bp = [0, 35, 45] + low_filter_time = 0.0 # No filtering under 35 mph + tuned_filter_time_curves = self.FILTER_TIME_CURVES[1] # At 35-55 mph + tuned_filter_time_leads = self.FILTER_TIME_LEADS[1] + tuned_filter_time_lights = self.FILTER_TIME_LIGHTS[1] + low_boost = 1.0 + tuned_boost = self.LIGHT_BOOSTS[1] + low_cap_factor = 0.0 # No cap under 35 mph + tuned_cap_factor = 1.0 + + filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves]) + filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads]) + filter_time_lights = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_lights]) + light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost]) + cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor]) + + # Update filter times with interp + self.curvature_filter = FirstOrderFilter(self.curvature_filter.x, filter_time_curves, DT_MDL) + self.slow_lead_filter = FirstOrderFilter(self.slow_lead_filter.x, filter_time_leads, DT_MDL) + self.stop_light_filter = FirstOrderFilter(self.stop_light_filter.x, filter_time_lights, DT_MDL) + + # Disable stoplight detection at very high speeds to prevent false positives + if speed_mph > 75: # Disable above 75 mph + self.stop_light_filter.x = 0 + self.stop_light_detected = False + return + + # Adjust model time with interp boost and gradual cap + adjusted_model_time = model_time * light_boost + if cap_factor > 0: + adjusted_model_time = min(adjusted_model_time, self.LIGHT_MAX_TIME * cap_factor + model_time * (1 - cap_factor)) # Gradual cap + + model_stopping = self.frogpilot_planner.model_length < v_ego * adjusted_model_time self.stop_light_filter.update(self.frogpilot_planner.model_stopped or model_stopping) - self.stop_light_detected = self.stop_light_filter.x >= THRESHOLD and not self.frogpilot_planner.tracking_lead + self.stop_light_detected = self.stop_light_filter.x >= THRESHOLD**2 and not self.frogpilot_planner.tracking_lead else: self.stop_light_filter.x = 0 self.stop_light_detected = False diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 6621a5252..0e2168183 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -1,33 +1,73 @@ #!/usr/bin/env python3 import numpy as np -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import CRUISE_MIN_ACCEL -from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel +def cubic_interp(x, xp, fp): + """Cubic interpolation using NumPy's native operations for speed.""" + # Boundary conditions + if x <= xp[0]: + return fp[0] + elif x >= xp[-1]: + return fp[-1] + + # Find interval + i = np.searchsorted(xp, x) - 1 + i = max(0, min(i, len(xp)-2)) # clamp the index + + # Normalized position + t = (x - xp[i]) / float(xp[i+1] - xp[i]) + + # Hermite cubic formula + return fp[i]*(1 - 3*t**2 + 2*t**3) + fp[i+1]*(3*t**2 - 2*t**3) + +def akima_interp(x, xp, fp): + """Akima-inspired interpolation with reduced overshoot characteristics.""" + if x <= xp[0]: + return fp[0] + elif x >= xp[-1]: + return fp[-1] + + i = np.searchsorted(xp, x) - 1 + i = max(0, min(i, len(xp)-2)) # clamp the index + + t = (x - xp[i]) / float(xp[i+1] - xp[i]) + + # Quintic polynomial to reduce overshoot + t2 = t*t + t4 = t2*t2 + t3 = t2*t + return (fp[i]*(1 - 10*t3 + 15*t4 - 6*t3*t2) + + fp[i+1]*(10*t3 - 15*t4 + 6*t3*t2)) + +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT -A_CRUISE_MIN_ECO = CRUISE_MIN_ACCEL / 2 -A_CRUISE_MIN_SPORT = CRUISE_MIN_ACCEL * 2 +A_CRUISE_MIN_ECO = A_CRUISE_MIN / 2 +A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2 - # MPH = [0.0, 11, 22, 34, 45, 56, 89] -A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] -A_CRUISE_MAX_VALS_ECO = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] -A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] + # MPH = [0.0, 11, 22, 34, 45, 56, 89] +A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] +A_CRUISE_MAX_VALS_ECO = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] +A_CRUISE_MAX_VALS_SPORT = [1.5, 1.5, 1.25, 1.5, 1.5, 1.5, 2.0] +A_CRUISE_MAX_VALS_SPORT_PLUS = [2.5, 2.5, 3.0, 2.5, 2.5, 2.5, 2.5] def get_max_accel_eco(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)) def get_max_accel_sport(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)) + +def get_max_accel_sport_plus(v_ego): + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)) def get_max_accel_low_speeds(max_accel, v_cruise): - return float(np.interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])) + return float(akima_interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])) def get_max_accel_ramp_off(max_accel, v_cruise, v_ego): - return float(np.interp(v_cruise - v_ego, [0., 1., 5.], [0., 0.5, max_accel])) + return float(akima_interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel])) def get_max_allowed_accel(v_ego): - return float(np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018 + return float(akima_interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018 class FrogPilotAcceleration: def __init__(self, FrogPilotPlanner): @@ -46,17 +86,17 @@ class FrogPilotAcceleration: if eco_gear: self.max_accel = get_max_accel_eco(v_ego) else: - if frogpilot_toggles.acceleration_profile == 2: - self.max_accel = get_max_accel_sport(v_ego) + if frogpilot_toggles.sport_plus: + self.max_accel = get_max_accel_sport_plus(v_ego) else: - self.max_accel = get_max_allowed_accel(v_ego) + self.max_accel = get_max_accel_sport(v_ego) else: if frogpilot_toggles.acceleration_profile == 1: self.max_accel = get_max_accel_eco(v_ego) elif frogpilot_toggles.acceleration_profile == 2: self.max_accel = get_max_accel_sport(v_ego) - elif frogpilot_toggles.acceleration_profile == 3: - self.max_accel = get_max_allowed_accel(v_ego) + elif frogpilot_toggles.sport_plus: + self.max_accel = get_max_accel_sport_plus(v_ego) else: self.max_accel = get_max_accel(v_ego) @@ -64,9 +104,7 @@ class FrogPilotAcceleration: self.max_accel = min(get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise), self.max_accel) self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel) - if self.frogpilot_planner.tracking_lead: - self.min_accel = ACCEL_MIN - elif sm["frogpilotCarState"].forceCoast: + if sm["frogpilotCarState"].forceCoast: self.min_accel = A_CRUISE_MIN_ECO elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear): if eco_gear: @@ -79,4 +117,4 @@ class FrogPilotAcceleration: elif frogpilot_toggles.deceleration_profile == 2: self.min_accel = A_CRUISE_MIN_SPORT else: - self.min_accel = CRUISE_MIN_ACCEL + self.min_accel = A_CRUISE_MIN diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index 688429037..8aefa244d 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import numpy as np -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT @@ -75,21 +75,20 @@ class FrogPilotFollowing: # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead if frogpilot_toggles.human_following and v_lead > v_ego: distance_factor = max(lead_distance - (v_ego * self.t_follow), 1) - accelerating_offset = float(np.clip(STOP_DISTANCE - v_ego, 1, distance_factor)) - - self.acceleration_jerk /= accelerating_offset - self.speed_jerk /= accelerating_offset - self.t_follow /= accelerating_offset + from frogpilot.common.frogpilot_variables import get_frogpilot_toggles + fp_toggles = get_frogpilot_toggles() + acceleration_offset = float(np.clip(fp_toggles.stop_distance - v_ego, 1, distance_factor)) + self.acceleration_jerk /= acceleration_offset + self.speed_jerk /= acceleration_offset + self.t_follow /= acceleration_offset # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead if (frogpilot_toggles.conditional_slower_lead or frogpilot_toggles.human_following) and v_lead < v_ego: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) braking_offset = float(np.clip(min(v_ego - v_lead, v_lead) - COMFORT_BRAKE, 1, distance_factor)) - if frogpilot_toggles.human_following: - if not self.following_lead and v_lead > CITY_SPEED_LIMIT: - far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - STOP_DISTANCE, 0) - else: - far_lead_offset = 0 + from frogpilot.common.frogpilot_variables import get_frogpilot_toggles + fp_toggles = get_frogpilot_toggles() + far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - fp_toggles.stop_distance, 0) self.t_follow /= braking_offset + far_lead_offset self.slower_lead = braking_offset > 1 diff --git a/frogpilot/navigation/mapd.py b/frogpilot/navigation/mapd.py index 9ccf5d70b..af9387db7 100644 --- a/frogpilot/navigation/mapd.py +++ b/frogpilot/navigation/mapd.py @@ -17,7 +17,7 @@ from openpilot.frogpilot.common.frogpilot_variables import MAPD_PATH, RESOURCES_ VERSION = "v2" GITHUB_VERSION_URL = f"https://github.com/{RESOURCES_REPO}/raw/Versions/mapd_version_{VERSION}.json" -GITLAB_VERSION_URL = f"https://gitlab.com/{RESOURCES_REPO}/-/raw/Versions/mapd_version_{VERSION}.json" +GITLAB_VERSION_URL = f"https://gitlab.com/firestar5683/FrogPilot-Resources/-/raw/Versions/mapd_version_{VERSION}.json" VERSION_PATH = Path("/data/media/0/osm/mapd_version") diff --git a/frogpilot/tinygrad_modeld/constants.py b/frogpilot/tinygrad_modeld/constants.py index e26773a55..0224ddb94 100644 --- a/frogpilot/tinygrad_modeld/constants.py +++ b/frogpilot/tinygrad_modeld/constants.py @@ -13,11 +13,15 @@ class ModelConstants: META_T_IDXS = [2., 4., 6., 8., 10.] # model inputs constants + MODEL_FREQ = 20 + HISTORY_FREQ = 5 + HISTORY_LEN_SECONDS = 5 + TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ + FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS + INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS N_FRAMES = 2 MODEL_RUN_FREQ = 20 MODEL_CONTEXT_FREQ = 5 # "model_trained_fps" - FULL_HISTORY_BUFFER_LEN = MODEL_RUN_FREQ * MODEL_CONTEXT_FREQ - TEMPORAL_SKIP = MODEL_RUN_FREQ // MODEL_CONTEXT_FREQ FEATURE_LEN = 512 diff --git a/frogpilot/tinygrad_modeld/fill_model_msg.py b/frogpilot/tinygrad_modeld/fill_model_msg.py index 926f0e24e..2cb5b0aa3 100644 --- a/frogpilot/tinygrad_modeld/fill_model_msg.py +++ b/frogpilot/tinygrad_modeld/fill_model_msg.py @@ -3,11 +3,26 @@ import capnp import numpy as np from cereal import log from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants, Plan, Meta +from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') ConfidenceClass = log.ModelDataV2.ConfidenceClass +# Return curvature for lateral action. If the model outputs desired_curvature and we're not in mlsim mode, +# use it directly; otherwise derive from the plan using yaw and yaw-rate. +def get_curvature_from_output(output: dict, v_ego: float, lat_action_t: float, mlsim: bool) -> float: + if not mlsim: + desired = output.get('desired_curvature') + if desired is not None: + return float(desired[0, 0]) + + plan_out = output['plan'][0] + # Use yaw (index 2) and yaw_rate (index 2) + theta = plan_out[:, Plan.T_FROM_CURRENT_EULER][:, 2] + theta_dot = plan_out[:, Plan.ORIENTATION_RATE][:, 2] + return float(get_curvature_from_plan(theta, theta_dot, ModelConstants.T_IDXS, v_ego, lat_action_t)) + class PublishState: def __init__(self): @@ -82,15 +97,32 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.timestampEof = timestamp_eof modelV2.modelExecutionTime = model_execution_time + # normalize plan tensors to (IDX_N, WIDTH) + plan_arr = net_output_data['plan'][0] + plan_stds_arr = net_output_data['plan_stds'][0] + # plan - fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T) - fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T) - fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T) - fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T) - fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) + fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *plan_arr[:,Plan.POSITION].T, *plan_stds_arr[:,Plan.POSITION].T) + fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *plan_arr[:,Plan.VELOCITY].T) + fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *plan_arr[:,Plan.ACCELERATION].T) + fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *plan_arr[:,Plan.T_FROM_CURRENT_EULER].T) + fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *plan_arr[:,Plan.ORIENTATION_RATE].T) + + # temporal pose + temporal_pose = modelV2.temporalPose + if 'sim_pose' in net_output_data: + temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() + temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() + temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist() + temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist() + else: + temporal_pose.trans = plan_arr[0,Plan.VELOCITY].tolist() + temporal_pose.transStd = plan_stds_arr[0,Plan.VELOCITY].tolist() + temporal_pose.rot = plan_arr[0,Plan.ORIENTATION_RATE].tolist() + temporal_pose.rotStd = plan_stds_arr[0,Plan.ORIENTATION_RATE].tolist() # poly path - fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) + fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *plan_arr[:,Plan.POSITION].T) # action modelV2.action = action diff --git a/frogpilot/tinygrad_modeld/parse_model_outputs.py b/frogpilot/tinygrad_modeld/parse_model_outputs.py index 3e77434d9..836f49046 100644 --- a/frogpilot/tinygrad_modeld/parse_model_outputs.py +++ b/frogpilot/tinygrad_modeld/parse_model_outputs.py @@ -1,13 +1,16 @@ import numpy as np from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants + def safe_exp(x, out=None): # -11 is around 10**14, more causes float16 overflow return np.exp(np.clip(x, -np.inf, 11), out=out) + def sigmoid(x): return 1. / (1. + safe_exp(-x)) + def softmax(x, axis=-1): x -= np.max(x, axis=axis, keepdims=True) if x.dtype == np.float32 or x.dtype == np.float64: @@ -17,15 +20,15 @@ def softmax(x, axis=-1): x /= np.sum(x, axis=axis, keepdims=True) return x + class Parser: def __init__(self, ignore_missing=False): self.ignore_missing = ignore_missing def check_missing(self, outs, name): - missing = name not in outs - if missing and not self.ignore_missing: + if name not in outs and not self.ignore_missing: raise ValueError(f"Missing output {name}") - return missing + return name not in outs def parse_categorical_crossentropy(self, name, outs, out_shape=None): if self.check_missing(outs, name): @@ -85,45 +88,50 @@ class Parser: outs[name] = pred_mu_final.reshape(final_shape) outs[name + '_stds'] = pred_std_final.reshape(final_shape) - def is_mhp(self, outs, name, shape): - if self.check_missing(outs, name): - return False - if outs[name].shape[1] == 2 * shape: - return False - return True + def split_outputs(self, outs: dict[str, np.ndarray]) -> None: + if 'lead' in outs: + if outs['lead'].shape[1] == 2 * ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH: + self.parse_mdn('lead', outs, in_N=0, out_N=0, + out_shape=(ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)) + else: + self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, + out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)) + if 'plan' in outs: + if outs['plan'].shape[1] == 2 * ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH: + self.parse_mdn('plan', outs, in_N=0, out_N=0, + out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) + else: + self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION, + out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) + if 'lane_lines' in outs: + self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, + out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH)) + if 'road_edges' in outs: + self.parse_mdn('road_edges', outs, in_N=0, out_N=0, + out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH)) + if 'sim_pose' in outs: + self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + if 'lane_lines_prob' in outs: + self.parse_binary_crossentropy('lane_lines_prob', outs) + if 'lead_prob' in outs: + self.parse_binary_crossentropy('lead_prob', outs) def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) - self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_binary_crossentropy('lane_lines_prob', outs) - self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) + self.split_outputs(outs) + self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH)) self.parse_binary_crossentropy('meta', outs) - self.parse_binary_crossentropy('lead_prob', outs) - lead_mhp = self.is_mhp(outs, 'lead', ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH) - lead_in_N, lead_out_N = (ModelConstants.LEAD_MHP_N, ModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0) - lead_out_shape = (ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) if lead_mhp else \ - (ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) - self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape) return outs def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: - plan_mhp = self.is_mhp(outs, 'plan', ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH) - plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0) - self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) - self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + self.split_outputs(outs) + if 'lat_planner_solution' in outs: + self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH)) if 'desired_curvature' in outs: self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,)) - for k in ['lead_prob', 'lane_lines_prob']: - self.parse_binary_crossentropy(k, outs) self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) - self.parse_binary_crossentropy('lead_prob', outs) - self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, - out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) return outs def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: diff --git a/frogpilot/tinygrad_modeld/tinygrad_modeld.py b/frogpilot/tinygrad_modeld/tinygrad_modeld.py index 59b2585d8..fee67f985 100755 --- a/frogpilot/tinygrad_modeld/tinygrad_modeld.py +++ b/frogpilot/tinygrad_modeld/tinygrad_modeld.py @@ -2,10 +2,6 @@ import os from openpilot.system.hardware import TICI os.environ['DEV'] = 'QCOM' if TICI else 'LLVM' -USBGPU = "USBGPU" in os.environ -if USBGPU: - os.environ['DEV'] = 'AMD' - os.environ['AMD_IFACE'] = 'USB' from tinygrad.tensor import Tensor from tinygrad.dtype import dtypes import time @@ -14,6 +10,7 @@ import numpy as np import cereal.messaging as messaging from cereal import car, log from pathlib import Path +from setproctitle import setproctitle from cereal.messaging import PubMaster, SubMaster from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog @@ -22,25 +19,21 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix +from openpilot.system import sentry from openpilot.selfdrive.car.car_helpers import get_demo_car_params from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import Parser -from openpilot.frogpilot.tinygrad_modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState +from openpilot.frogpilot.tinygrad_modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants, Plan from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext from openpilot.frogpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address - -from openpilot.frogpilot.common.frogpilot_variables import MODELS_PATH, get_frogpilot_toggles +from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles, MODELS_PATH PROCESS_NAME = "frogpilot.tinygrad_modeld.tinygrad_modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -VISION_PKL_PATH = Path(__file__).parent / 'models/driving_vision_tinygrad.pkl' -POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' -VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' -POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' LAT_SMOOTH_SECONDS = 0.1 LONG_SMOOTH_SECONDS = 0.3 @@ -48,23 +41,22 @@ MIN_LAT_CONTROL_SPEED = 0.3 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, - lat_action_t: float, long_action_t: float, v_ego: float, use_curvature_from_plan: bool) -> log.ModelDataV2.Action: + lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, is_v9: bool) -> log.ModelDataV2.Action: plan = model_output['plan'][0] - desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], - plan[:,Plan.ACCELERATION][:,0], - ModelConstants.T_IDXS, - action_t=long_action_t) + desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0], + plan[:,Plan.ACCELERATION][:,0], + ModelConstants.T_IDXS, + action_t=long_action_t) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) - if use_curvature_from_plan: - desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2], - plan[:,Plan.ORIENTATION_RATE][:,2], - ModelConstants.T_IDXS, - v_ego, - lat_action_t) + if is_v9: + # V9: use desired_curvature if present; otherwise do NOT fall back to plan + if 'desired_curvature' in model_output: + desired_curvature = float(model_output['desired_curvature'][0, 0]) + else: + desired_curvature = prev_action.desiredCurvature else: - desired_curvature = model_output['desired_curvature'][0, 0] - + desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, mlsim=mlsim) if v_ego > MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) else: @@ -83,113 +75,138 @@ class FrameMeta: if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof -class InputQueues: - def __init__ (self, model_fps, env_fps, n_frames_input): - assert env_fps % model_fps == 0 - assert env_fps >= model_fps - self.model_fps = model_fps - self.env_fps = env_fps - self.n_frames_input = n_frames_input - - self.dtypes = {} - self.shapes = {} - self.q = {} - - def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None: - self.dtypes.update(input_dtypes) - if self.env_fps == self.model_fps: - self.shapes.update(input_shapes) - else: - for k in input_shapes: - shape = list(input_shapes[k]) - if 'img' in k: - n_channels = shape[1] // self.n_frames_input - shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels - else: - shape[1] = (self.env_fps // self.model_fps) * shape[1] - self.shapes[k] = tuple(shape) - - def reset(self) -> None: - self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()} - - def enqueue(self, inputs:dict[str, np.ndarray]) -> None: - for k in inputs.keys(): - if inputs[k].dtype != self.dtypes[k]: - raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}') - input_shape = list(self.shapes[k]) - input_shape[1] = -1 - single_input = inputs[k].reshape(tuple(input_shape)) - sz = single_input.shape[1] - self.q[k][:,:-sz] = self.q[k][:,sz:] - self.q[k][:,-sz:] = single_input - - def get(self, *names) -> dict[str, np.ndarray]: - if self.env_fps == self.model_fps: - return {k: self.q[k] for k in names} - else: - out = {} - for k in names: - shape = self.shapes[k] - if 'img' in k: - n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1)) - out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1) - elif 'pulse' in k: - # any pulse within interval counts - out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2) - else: - idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1] - out[k] = self.q[k][:, idxs] - return out - class ModelState: frames: dict[str, DrivingModelFrame] inputs: dict[str, np.ndarray] output: np.ndarray prev_desire: np.ndarray # for tracking the rising edge of the pulse - def __init__(self, context: CLContext, model: str): - with open(MODELS_PATH / f'{model}_driving_vision_metadata.pkl', 'rb') as f: - vision_metadata = pickle.load(f) - self.vision_input_shapes = vision_metadata['input_shapes'] - self.vision_input_names = list(self.vision_input_shapes.keys()) - self.vision_output_slices = vision_metadata['output_slices'] - vision_output_size = vision_metadata['output_shapes']['outputs'][1] + def __init__(self, context: CLContext): + # Dynamically build paths based on current model ID + params = Params() + model_id = params.get("Model", encoding="utf-8") - with open(MODELS_PATH / f'{model}_driving_policy_metadata.pkl', 'rb') as f: - policy_metadata = pickle.load(f) - self.policy_input_shapes = policy_metadata['input_shapes'] - self.policy_output_slices = policy_metadata['output_slices'] - policy_output_size = policy_metadata['output_shapes']['outputs'][1] + # Try to get ModelVersion, but handle case where parameter doesn't exist + model_version = None + try: + model_version = params.get("ModelVersion", encoding="utf-8") + except Exception as e: + cloudlog.warning(f"ModelVersion parameter not available: {e}") - self.desire_type = 'desire_pulse' if 'desire_pulse' in self.policy_input_shapes else 'desire' - self.use_lateral_control_params = 'lateral_control_params' in self.policy_input_shapes + model_dir = MODELS_PATH + VISION_PKL_PATH = model_dir / f"{model_id}_driving_vision_tinygrad.pkl" + POLICY_PKL_PATH = model_dir / f"{model_id}_driving_policy_tinygrad.pkl" + VISION_METADATA_PATH = model_dir / f"{model_id}_driving_vision_metadata.pkl" + POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_policy_metadata.pkl" - self.frames = {name: DrivingModelFrame(context, ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ) for name in self.vision_input_names} + # If ModelVersion is not set or not available, try to determine it from available model data + if not model_version: + cloudlog.warning(f"ModelVersion not available for model {model_id}, attempting to determine from model data") + try: + # Try to get version from the model versions JSON file + versions_file = model_dir / ".model_versions.json" + if versions_file.is_file(): + import json + with open(versions_file, "r") as f: + version_map = json.load(f) + if model_id in version_map: + model_version = version_map[model_id] + cloudlog.warning(f"Determined model version from JSON: {model_version}") + else: + cloudlog.error("Model versions JSON file not found, defaulting to v8") + model_version = "v8" + except Exception as e: + cloudlog.error(f"Failed to determine model version: {e}, defaulting to v8") + model_version = "v8" + + try: + with open(VISION_METADATA_PATH, 'rb') as f: + vision_metadata = pickle.load(f) + except FileNotFoundError: + cloudlog.error(f"Missing metadata {VISION_METADATA_PATH}, downloading...") + from openpilot.frogpilot.assets.model_manager import ModelManager + ModelManager().download_model(model_id) + with open(VISION_METADATA_PATH, 'rb') as f: + vision_metadata = pickle.load(f) + self.vision_input_shapes = vision_metadata['input_shapes'] + self.vision_input_names = list(self.vision_input_shapes.keys()) + self.vision_output_slices = vision_metadata['output_slices'] + vision_output_size = vision_metadata['output_shapes']['outputs'][1] + + try: + with open(POLICY_METADATA_PATH, 'rb') as f: + policy_metadata = pickle.load(f) + except FileNotFoundError: + cloudlog.error(f"Missing metadata {POLICY_METADATA_PATH}, downloading...") + from openpilot.frogpilot.assets.model_manager import ModelManager + ModelManager().download_model(model_id) + with open(POLICY_METADATA_PATH, 'rb') as f: + policy_metadata = pickle.load(f) + self.policy_input_shapes = policy_metadata['input_shapes'] + self.policy_output_slices = policy_metadata['output_slices'] + policy_output_size = policy_metadata['output_shapes']['outputs'][1] + # Add policy_generation attribute after loading policy_metadata + self.policy_generation = model_version or "v8" + self.is_v11 = (self.policy_generation == "v11") + self.is_v9 = (self.policy_generation == "v9") + self.mlsim = (self.policy_generation in ("v8", "v10", "v11")) + + self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names} self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) - self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.FULL_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP) + self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) + self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32) + self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP) + + + # policy inputs (built dynamically to support all generations) + self.numpy_inputs = {} + + # Always-supported inputs (if model expects them) + desire_key_init = next((k for k in self.policy_input_shapes if k.startswith('desire')), None) + if desire_key_init: + self.numpy_inputs[desire_key_init] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32) + if 'traffic_convention' in self.policy_input_shapes: + self.numpy_inputs['traffic_convention'] = np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32) + if 'features_buffer' in self.policy_input_shapes: + self.numpy_inputs['features_buffer'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) + + # Optional inputs for non-v11 (and some v10/v9 variants) + # Lateral control params + if 'lateral_control_params' in self.policy_input_shapes: + self.numpy_inputs['lateral_control_params'] = np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32) + + # Previous desired curvature: handle both singular and plural key names across model versions + self.prev_desired_curv_key = None + if 'prev_desired_curv' in self.policy_input_shapes: + self.prev_desired_curv_key = 'prev_desired_curv' + self.numpy_inputs['prev_desired_curv'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) + elif 'prev_desired_curvs' in self.policy_input_shapes: + self.prev_desired_curv_key = 'prev_desired_curvs' + self.numpy_inputs['prev_desired_curvs'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) + + # Optional temporal buffer for previous desired curvature (allocate only if the policy expects it) + if getattr(self, 'prev_desired_curv_key', None) is not None: + self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) - # policy inputs - self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes} - self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES) - for k in [self.desire_type, 'features_buffer']: - self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape}) - self.full_input_queues.reset() # img buffers are managed in openCL transform code self.vision_inputs: dict[str, Tensor] = {} self.vision_output = np.zeros(vision_output_size, dtype=np.float32) self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self.policy_output = np.zeros(policy_output_size, dtype=np.float32) - self.parser = Parser(ignore_missing=True) + self.parser = Parser() - with open(MODELS_PATH / f'{model}_driving_vision_tinygrad.pkl', "rb") as f: + with open(VISION_PKL_PATH, "rb") as f: self.vision_run = pickle.load(f) - with open(MODELS_PATH / f'{model}_driving_policy_tinygrad.pkl', "rb") as f: + with open(POLICY_PKL_PATH, "rb") as f: self.policy_run = pickle.load(f) + @property + def desire_key(self) -> str: + return next(key for key in self.numpy_inputs if key.startswith('desire')) + def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()} return parsed_model_outputs @@ -197,15 +214,24 @@ class ModelState: def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge - inputs[self.desire_type][0] = 0 - new_desire = np.where(inputs[self.desire_type] - self.prev_desire > .99, inputs[self.desire_type], 0) - self.prev_desire[:] = inputs[self.desire_type] + inputs[self.desire_key][0] = 0 + new_desire = np.where(inputs[self.desire_key] - self.prev_desire > .99, inputs[self.desire_key], 0) + self.prev_desire[:] = inputs[self.desire_key] - if self.use_lateral_control_params: + self.full_desire[0,:-1] = self.full_desire[0,1:] + self.full_desire[0,-1] = new_desire + self.numpy_inputs[self.desire_key][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2) + + self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] + if 'lateral_control_params' in self.numpy_inputs: self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] + + if prepare_only: + return None + imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names} - if TICI and not USBGPU: + if TICI: # The imgs tensors are backed by opencl memory, only need init once for key in imgs_cl: if key not in self.vision_inputs: @@ -215,25 +241,27 @@ class ModelState: frame_input = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.vision_input_shapes[key]) self.vision_inputs[key] = Tensor(frame_input, dtype=dtypes.uint8).realize() - if prepare_only: - return None - self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy() vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices)) - self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], self.desire_type: new_desire}) - for k in [self.desire_type, 'features_buffer']: - self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k] - self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] + self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:] + self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :] + self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs] self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy() policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) - if self.use_lateral_control_params: - # TODO model only uses last value now + # TODO model only uses last value now + if hasattr(self, 'full_prev_desired_curv') and 'desired_curvature' in policy_outputs_dict: self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:] self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :] - self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs] + + if self.prev_desired_curv_key is not None: + # v9 models expect zeros for prev_desired_curv(s); others use history + if self.is_v9: + self.numpy_inputs[self.prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs] + else: + self.numpy_inputs[self.prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs] combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} if SEND_RAW_PRED: @@ -243,26 +271,18 @@ class ModelState: def main(demo=False): - # FrogPilot variables - frogpilot_toggles = get_frogpilot_toggles() + cloudlog.warning("modeld init") - model_name = frogpilot_toggles.model - model_version = frogpilot_toggles.model_version - use_curvature_from_plan = frogpilot_toggles.model_version != "v7" + sentry.set_tag("daemon", PROCESS_NAME) + cloudlog.bind(daemon=PROCESS_NAME) + setproctitle(PROCESS_NAME) + config_realtime_process(7, 54) - cloudlog.warning("tinygrad_modeld init") - - if not USBGPU: - # USB GPU currently saturates a core so can't do this yet, - # also need to move the aux USB interrupts for good timings - config_realtime_process(7, 54) - - st = time.monotonic() cloudlog.warning("setting up CL context") cl_context = CLContext() cloudlog.warning("CL context ready; loading model") - model = ModelState(cl_context, model_name) - cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, tinygrad_modeld starting") + model = ModelState(cl_context) + cloudlog.warning("models loaded, modeld starting") # visionipc clients while True: @@ -295,7 +315,7 @@ def main(demo=False): params = Params() # setup filter to track dropped frames - frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ) + frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ) frame_id = 0 last_vipc_frame_id = 0 run_count = 0 @@ -322,6 +342,9 @@ def main(demo=False): DH = DesireHelper() + # FrogPilot variables + frogpilot_toggles = get_frogpilot_toggles() + while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: @@ -391,11 +414,14 @@ def main(demo=False): bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names} transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names} + inputs:dict[str, np.ndarray] = { - model.desire_type: vec_desire, + model.desire_key: vec_desire, 'traffic_convention': traffic_convention, - **({'lateral_control_params': lateral_control_params} if model.use_lateral_control_params else {}), } + # Include optional inputs only if the loaded model expects them + if 'lateral_control_params' in model.numpy_inputs: + inputs['lateral_control_params'] = lateral_control_params mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) @@ -408,7 +434,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, use_curvature_from_plan) + action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, model.mlsim, model.is_v9) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, @@ -432,7 +458,7 @@ def main(demo=False): pm.send('cameraOdometry', posenet_send) last_vipc_frame_id = meta_main.frame_id - # Update FrogPilot variables + # Update FrogPilot parameters if sm['frogpilotPlan'].togglesUpdated: frogpilot_toggles = get_frogpilot_toggles() @@ -444,4 +470,7 @@ if __name__ == "__main__": args = parser.parse_args() main(demo=args.demo) except KeyboardInterrupt: - cloudlog.warning("got SIGINT") + cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") + except Exception: + sentry.capture_exception() + raise \ No newline at end of file diff --git a/frogpilot/ui/qt/offroad/expandable_multi_option_dialog.cc b/frogpilot/ui/qt/offroad/expandable_multi_option_dialog.cc new file mode 100644 index 000000000..e545045b7 --- /dev/null +++ b/frogpilot/ui/qt/offroad/expandable_multi_option_dialog.cc @@ -0,0 +1,206 @@ +#include "frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/scrollview.h" + +ExpandableMultiOptionDialog::ExpandableMultiOptionDialog(const QString &prompt_text, + const QMap &seriesToModels, + const QString ¤t, QWidget *parent) + : DialogBase(parent), seriesToModels(seriesToModels) { + + QFrame *container = new QFrame(this); + container->setStyleSheet(R"( + QFrame { background-color: #1B1B1B; } + QPushButton { + height: 135; + padding: 0px 50px; + text-align: left; + font-size: 55px; + font-weight: 300; + border-radius: 10px; + background-color: #4F4F4F; + border: 2px solid transparent; + } + QPushButton.model-option:checked { + background-color: #465BEA !important; + border: 3px solid #FFFFFF !important; + color: white !important; + font-weight: 500 !important; + } + QPushButton:hover { background-color: #5A5A5A; } + QPushButton.model-option:checked:hover { background-color: #5A6BEA; } + QPushButton:pressed { + background-color: #3049F4; + } + QPushButton.model-option:checked:pressed { + background-color: #3049F4; + border: 3px solid #CCCCCC; + } + QPushButton.series-header { + background-color: #333333; + font-weight: 500; + text-align: left; + padding-left: 80px; + } + QPushButton.series-header:hover { background-color: #404040; } + )"); + + QVBoxLayout *main_layout = new QVBoxLayout(container); + main_layout->setContentsMargins(55, 50, 55, 50); + + QLabel *title = new QLabel(prompt_text, this); + title->setStyleSheet("font-size: 70px; font-weight: 500;"); + main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); + main_layout->addSpacing(25); + + QWidget *listWidget = new QWidget(this); + QVBoxLayout *listLayout = new QVBoxLayout(listWidget); + listLayout->setSpacing(10); + + QButtonGroup *group = new QButtonGroup(listWidget); + group->setExclusive(true); + + QPushButton *confirm_btn = new QPushButton(tr("Select")); + confirm_btn->setObjectName("confirm_btn"); + confirm_btn->setEnabled(false); + + ScrollView *scroll_view = new ScrollView(listWidget, this); + scroll_view->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + + // Create series headers and their expandable content + for (const QString &series : seriesToModels.keys()) { + // Series header button + QPushButton *seriesHeader = new QPushButton("β–Ά " + series); + seriesHeader->setProperty("class", "series-header"); + seriesHeader->setCheckable(false); + seriesExpanded[series] = false; + + QObject::connect(seriesHeader, &QPushButton::clicked, [this, series, seriesHeader, scroll_view]() { + toggleSeries(series, seriesHeader, scroll_view); + }); + + listLayout->addWidget(seriesHeader); + + // Container for series models (initially hidden) + QWidget *seriesContainer = new QWidget(); + QVBoxLayout *seriesLayout = new QVBoxLayout(seriesContainer); + seriesLayout->setContentsMargins(20, 0, 0, 0); + seriesLayout->setSpacing(10); + seriesContainer->hide(); + + // Add models for this series + for (const QString &model : seriesToModels[series]) { + QPushButton *modelButton = new QPushButton(model); + modelButton->setCheckable(true); + modelButton->setChecked(model == current); + modelButton->setProperty("class", "model-option"); + + QObject::connect(modelButton, &QPushButton::toggled, [=](bool checked) mutable { + if (checked) { + selection = model; + confirm_btn->setEnabled(true); + // Manually apply selected style + modelButton->setStyleSheet("QPushButton {" + "background-color: #465BEA;" + "border: 3px solid #FFFFFF;" + "color: white;" + "font-weight: 500;" + "height: 135;" + "padding: 0px 50px;" + "text-align: left;" + "font-size: 55px;" + "border-radius: 10px;" + "}"); + } else { + if (selection == model) { + confirm_btn->setEnabled(false); + } + // Reset to default style + modelButton->setStyleSheet(""); + } + }); + + group->addButton(modelButton); + seriesLayout->addWidget(modelButton); + } + + seriesWidgets[series] = seriesContainer; + listLayout->addWidget(seriesContainer); + } + + // Add stretch to keep buttons spaced correctly + listLayout->addStretch(1); + + main_layout->addWidget(scroll_view); + main_layout->addSpacing(35); + + // Cancel + confirm buttons + QHBoxLayout *blayout = new QHBoxLayout; + main_layout->addLayout(blayout); + blayout->setSpacing(50); + + QPushButton *cancel_btn = new QPushButton(tr("Cancel")); + QObject::connect(cancel_btn, &QPushButton::clicked, this, &ConfirmationDialog::reject); + QObject::connect(confirm_btn, &QPushButton::clicked, this, &ConfirmationDialog::accept); + blayout->addWidget(cancel_btn); + blayout->addWidget(confirm_btn); + + QVBoxLayout *outer_layout = new QVBoxLayout(this); + outer_layout->setContentsMargins(50, 50, 50, 50); + outer_layout->addWidget(container); +} + +void ExpandableMultiOptionDialog::toggleSeries(const QString &series, QPushButton *headerButton, ScrollView *scrollView) { + bool expanded = seriesExpanded[series]; + QWidget *container = seriesWidgets[series]; + QString seriesName = series; + + if (expanded) { + container->hide(); + seriesExpanded[series] = false; + headerButton->setText("β–Ά " + seriesName); + } else { + container->show(); + seriesExpanded[series] = true; + headerButton->setText("β–Ό " + seriesName); + + // Auto-scroll to show expanded content + if (scrollView) { + QTimer::singleShot(50, [container, scrollView]() { + QRect containerRect = container->geometry(); + QScrollBar *vScrollBar = scrollView->verticalScrollBar(); + if (vScrollBar) { + int currentValue = vScrollBar->value(); + int containerBottom = containerRect.bottom(); + int viewportHeight = scrollView->viewport()->height(); + + // If container extends beyond viewport, scroll to show it + if (containerBottom > currentValue + viewportHeight) { + int targetValue = containerBottom - viewportHeight + 50; // Add some padding + vScrollBar->setValue(targetValue); + } + } + }); + } + } + + // Update the button's appearance + headerButton->update(); +} + +QString ExpandableMultiOptionDialog::getSelection(const QString &prompt_text, + const QMap &seriesToModels, + const QString ¤t, QWidget *parent) { + ExpandableMultiOptionDialog d = ExpandableMultiOptionDialog(prompt_text, seriesToModels, current, parent); + if (d.exec()) { + return d.selection; + } + return ""; +} \ No newline at end of file diff --git a/frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h b/frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h new file mode 100644 index 000000000..e89a0101e --- /dev/null +++ b/frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/input.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + +class ExpandableMultiOptionDialog : public DialogBase { + Q_OBJECT + +public: + explicit ExpandableMultiOptionDialog(const QString &prompt_text, const QMap &seriesToModels, + const QString ¤t, QWidget *parent); + static QString getSelection(const QString &prompt_text, const QMap &seriesToModels, + const QString ¤t, QWidget *parent); + QString selection; + +private: + void toggleSeries(const QString &series, QPushButton *headerButton, ScrollView *scrollView); + QMap seriesToModels; + QMap seriesWidgets; + QMap seriesExpanded; +}; \ No newline at end of file diff --git a/frogpilot/ui/qt/offroad/model_settings.cc b/frogpilot/ui/qt/offroad/model_settings.cc index 815a73007..669f5382d 100644 --- a/frogpilot/ui/qt/offroad/model_settings.cc +++ b/frogpilot/ui/qt/offroad/model_settings.cc @@ -1,39 +1,12 @@ #include "frogpilot/ui/qt/offroad/model_settings.h" - -bool hasAllTinygradFiles(const QDir &modelDir, const QString &modelKey) { - QStringList tinygradSuffixes = { - "_driving_policy_metadata.pkl", - "_driving_policy_tinygrad.pkl", - "_driving_vision_metadata.pkl", - "_driving_vision_tinygrad.pkl" - }; - - for (const QString &suffix : tinygradSuffixes) { - if (!modelDir.exists(modelKey + suffix)) { - return false; - } - } - return true; -} - -QString normalizeModelKey(QString key) { - key = key.toLower(); - if (key.endsWith("_default")) { - key.chop(QString("_default").size()); - } - return key; -} +#include "frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h" +#include +#include +#include +#include +#include FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : FrogPilotListWidget(parent), parent(parent) { - QJsonObject shownDescriptions = QJsonDocument::fromJson(QString::fromStdString(params.get("ShownToggleDescriptions")).toUtf8()).object(); - QString className = this->metaObject()->className(); - - if (!shownDescriptions.value(className).toBool(false)) { - forceOpenDescriptions = true; - shownDescriptions.insert(className, true); - params.put("ShownToggleDescriptions", QJsonDocument(shownDescriptions).toJson(QJsonDocument::Compact).toStdString()); - } - QStackedLayout *modelLayout = new QStackedLayout(); addItem(modelLayout); @@ -50,16 +23,18 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog modelLayout->addWidget(modelLabelsPanel); const std::vector> modelToggles { - {"AutomaticallyDownloadModels", tr("Automatically Download New Models"), tr("Automatically download new driving models as they become available."), ""}, - {"DeleteModel", tr("Delete Driving Models"), tr("Delete downloaded driving models to free up storage space."), ""}, - {"DownloadModel", tr("Download Driving Models"), tr("Manually download driving models to the device."), ""}, - {"ModelRandomizer", tr("Model Randomizer"), tr("Select a random driving model each drive and use feedback prompts at the end of the drive to help find the model that best suits you!"), ""}, - {"ManageBlacklistedModels", tr("Manage Model Blacklist"), tr("Add or remove driving models from the \"Model Randomizer\" blacklist."), ""}, - {"ManageScores", tr("Manage Model Ratings"), tr("View or reset saved model ratings used by the \"Model Randomizer\"."), ""}, - {"SelectModel", tr("Select Driving Model"), tr("Choose which driving model openpilot uses."), ""}, - {"UpdateTinygrad", tr("Update Model Manager"), tr("Update the \"Model Manager\" to support the latest models."), ""} + {"AutomaticallyDownloadModels", tr("Automatically Download New Models"), tr("Automatically download new driving models as they become available."), ""}, + {"DeleteModel", tr("Delete Driving Models"), tr("Delete driving models from the device."), ""}, + {"DownloadModel", tr("Download Driving Models"), tr("Download driving models to the device."), ""}, + {"ModelRandomizer", tr("Model Randomizer"), tr("Driving models are chosen at random each drive and feedback prompts are used to find the model that best suits your needs."), ""}, + {"StopDistance", tr("Stop Distance"), tr("Adjust the model's stopping distance in meters (minimum 4 for safety). Most users prefer 6."), ""}, + {"ManageBlacklistedModels", tr("Manage Model Blacklist"), tr("Add or remove models from the Model Randomizer's blacklist list."), ""}, + {"ManageScores", tr("Manage Model Ratings"), tr("Reset or view the saved ratings for the driving models."), ""}, + {"SelectModel", tr("Select Driving Model"), tr("Select the active driving model."), ""}, }; + FrogPilotParamValueButtonControl *stopDistanceToggle = nullptr; + for (const auto &[param, title, desc, icon] : modelToggles) { AbstractControl *modelToggle; @@ -79,11 +54,25 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } } deletableModels.removeAll(processModelName(currentModel)); - deletableModels.removeAll(modelFileToNameMapProcessed.value(normalizeModelKey(QString::fromStdString(params_default.get("Model"))))); + deletableModels.removeAll(modelFileToNameMapProcessed.value(QString::fromStdString(params_default.get("Model")))); + deletableModels.removeAll("Space Lab"); noModelsDownloaded = deletableModels.isEmpty(); if (id == 0) { - QString modelToDelete = MultiOptionDialog::getSelection(tr("Select a driving model to delete"), deletableModels, "", this); + // Group deletable models by series + QMap deletableSeriesToModels; + for (const QString &modelName : deletableModels) { + QString modelKey = modelFileToNameMapProcessed.key(modelName); + QString series = modelSeriesMap.value(modelKey, "Custom Series"); + deletableSeriesToModels[series].append(modelName); + } + + // Sort models within each series + for (QString &series : deletableSeriesToModels.keys()) { + deletableSeriesToModels[series].sort(); + } + + QString modelToDelete = ExpandableMultiOptionDialog::getSelection(tr("Select a driving model to delete"), deletableSeriesToModels, "", this); if (!modelToDelete.isEmpty() && ConfirmationDialog::confirm(tr("Are you sure you want to delete the \"%1\" model?").arg(modelToDelete), tr("Delete"), this)) { QString modelFile = modelFileToNameMapProcessed.key(modelToDelete); for (const QString &file : modelDir.entryList(QDir::Files)) { @@ -117,19 +106,37 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } else if (param == "DownloadModel") { downloadModelButton = new FrogPilotButtonsControl(title, desc, icon, {tr("DOWNLOAD"), tr("DOWNLOAD ALL")}); QObject::connect(downloadModelButton, &FrogPilotButtonsControl::buttonClicked, [this](int id) { - if (tinygradUpdate) { - if (FrogPilotConfirmationDialog::yesorno(tr("Tinygrad is out of date and must be updated before you can download new models. Update now?"), this)) { - if (FrogPilotConfirmationDialog::yesorno(tr("Updating Tinygrad will delete all existing Tinygrad-based models which will need to be re-downloaded. Proceed?"), this)) { - params_memory.putBool("UpdateTinygrad", true); - params_memory.put("ModelDownloadProgress", "Downloading..."); + auto isInstalled = [this](const QString &key) { + bool has_thneed = false; + bool has_policy_meta = false; + bool has_policy_tg = false; + bool has_vision_meta = false; + bool has_vision_tg = false; - updateTinygradButton->setText(0, tr("CANCEL")); - updateTinygradButton->setValue(tr("Updating...")); + for (const QString &file : modelDir.entryList(QDir::Files)) { + QFileInfo fi(modelDir.filePath(file)); + const QString base = fi.baseName(); + const QString ext = fi.suffix(); + if (!(base.startsWith(key) || base.startsWith(key + "_"))) continue; - updatingTinygrad = true; + if (ext == "thneed") { + // Classic model (WD-40 etc.) + has_thneed = true; + } else if (ext == "pkl") { + // TinyGrad bundle uses these four exact suffixes + if (base.contains("_driving_policy_metadata")) has_policy_meta = true; + else if (base.contains("_driving_policy_tinygrad")) has_policy_tg = true; + else if (base.contains("_driving_vision_metadata")) has_vision_meta = true; + else if (base.contains("_driving_vision_tinygrad")) has_vision_tg = true; } } - } else if (id == 0) { + + // Classic models: any matching .thneed counts as installed + if (has_thneed) return true; + // TinyGrad models: require all four policy/vision files to be present + return has_policy_meta && has_policy_tg && has_vision_meta && has_vision_tg; + }; + if (id == 0) { if (modelDownloading) { params_memory.putBool("CancelModelDownload", true); @@ -138,15 +145,43 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog QStringList downloadableModels = availableModelNames; for (const QString &modelKey : modelFileToNameMap.keys()) { QString modelName = modelFileToNameMap.value(modelKey); - if (modelDir.exists(modelKey + ".thneed") || hasAllTinygradFiles(modelDir, modelKey)) { + if (isInstalled(modelKey)) { downloadableModels.removeAll(modelName); } } + downloadableModels.removeAll("Space Lab πŸ‘€πŸ“‘"); allModelsDownloaded = downloadableModels.isEmpty(); - QString modelToDownload = MultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableModels, "", this); + // Group downloadable models by series + QMap downloadableSeriesToModels; + for (const QString &modelName : downloadableModels) { + QString modelKey = modelFileToNameMap.key(modelName); + QString series = modelSeriesMap.value(modelKey, "Custom Series"); + downloadableSeriesToModels[series].append(modelName); + } + + // Sort models within each series + for (QString &series : downloadableSeriesToModels.keys()) { + downloadableSeriesToModels[series].sort(); + } + + QString modelToDownload = ExpandableMultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableSeriesToModels, "", this); if (!modelToDownload.isEmpty()) { - params_memory.put("ModelToDownload", modelFileToNameMap.key(modelToDownload).toStdString()); + QString modelKey = modelFileToNameMap.key(modelToDownload); + params_memory.put("ModelToDownload", modelKey.toStdString()); + // Also persist the version for this downloaded model if known + { + QFile vf("/data/models/.model_versions.json"); + if (vf.open(QIODevice::ReadOnly)) { + auto doc = QJsonDocument::fromJson(vf.readAll()); + if (doc.isObject()) { + auto obj = doc.object(); + if (obj.contains(modelKey)) { + params.put("ModelVersion", obj.value(modelKey).toString().toStdString()); + } + } + } + } params_memory.put("ModelDownloadProgress", "Downloading..."); downloadModelButton->setText(0, tr("CANCEL")); @@ -179,8 +214,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog }); modelToggle = downloadModelButton; } else if (param == "ManageBlacklistedModels") { - FrogPilotButtonsControl *blacklistButton = new FrogPilotButtonsControl(title, desc, icon, {tr("ADD"), tr("REMOVE"), tr("REMOVE ALL")}); - QObject::connect(blacklistButton, &FrogPilotButtonsControl::buttonClicked, [this](int id) { + FrogPilotButtonsControl *blacklistBtn = new FrogPilotButtonsControl(title, desc, icon, {tr("ADD"), tr("REMOVE"), tr("REMOVE ALL")}); + QObject::connect(blacklistBtn, &FrogPilotButtonsControl::buttonClicked, [this](int id) { QStringList blacklistedModels = QString::fromStdString(params.get("BlacklistedModels")).split(","); blacklistedModels.removeAll(""); @@ -193,9 +228,22 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } if (blacklistableModels.size() <= 1) { - ConfirmationDialog::alert(tr("There are no more driving models to blacklist. The only available model is \"%1\"!").arg(blacklistableModels.first()), this); + ConfirmationDialog::alert(tr("There are no more models to blacklist! The only available model is \"%1\"!").arg(blacklistableModels.first()), this); } else { - QString modelToBlacklist = MultiOptionDialog::getSelection(tr("Select a driving model to add to the blacklist"), blacklistableModels, "", this); + // Group blacklistable models by series + QMap blacklistableSeriesToModels; + for (const QString &modelName : blacklistableModels) { + QString modelKey = modelFileToNameMapProcessed.key(modelName); + QString series = modelSeriesMap.value(modelKey, "Custom Series"); + blacklistableSeriesToModels[series].append(modelName); + } + + // Sort models within each series + for (QString &series : blacklistableSeriesToModels.keys()) { + blacklistableSeriesToModels[series].sort(); + } + + QString modelToBlacklist = ExpandableMultiOptionDialog::getSelection(tr("Select a model to add to the blacklist"), blacklistableSeriesToModels, "", this); if (!modelToBlacklist.isEmpty()) { if (ConfirmationDialog::confirm(tr("Are you sure you want to add the \"%1\" model to the blacklist?").arg(modelToBlacklist), tr("Add"), this)) { blacklistedModels.append(modelFileToNameMapProcessed.key(modelToBlacklist)); @@ -210,9 +258,21 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog QString modelName = modelFileToNameMapProcessed.value(model); whitelistableModels.append(modelName); } - whitelistableModels.sort(); - QString modelToWhitelist = MultiOptionDialog::getSelection(tr("Select a driving model to remove from the blacklist"), whitelistableModels, "", this); + // Group whitelistable models by series + QMap whitelistableSeriesToModels; + for (const QString &modelName : whitelistableModels) { + QString modelKey = modelFileToNameMapProcessed.key(modelName); + QString series = modelSeriesMap.value(modelKey, "Custom Series"); + whitelistableSeriesToModels[series].append(modelName); + } + + // Sort models within each series + for (QString &series : whitelistableSeriesToModels.keys()) { + whitelistableSeriesToModels[series].sort(); + } + + QString modelToWhitelist = ExpandableMultiOptionDialog::getSelection(tr("Select a model to remove from the blacklist"), whitelistableSeriesToModels, "", this); if (!modelToWhitelist.isEmpty()) { if (ConfirmationDialog::confirm(tr("Are you sure you want to remove the \"%1\" model from the blacklist?").arg(modelToWhitelist), tr("Remove"), this)) { blacklistedModels.removeAll(modelFileToNameMapProcessed.key(modelToWhitelist)); @@ -221,18 +281,18 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } } } else if (id == 2) { - if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure you want to remove all of your blacklisted driving models?"), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure you want to remove all of your blacklisted models?"), this)) { params.remove("BlacklistedModels"); params_cache.remove("BlacklistedModels"); } } }); - modelToggle = blacklistButton; + modelToggle = blacklistBtn; } else if (param == "ManageScores") { - FrogPilotButtonsControl *manageScoresButton = new FrogPilotButtonsControl(title, desc, icon, {tr("RESET"), tr("VIEW")}); - QObject::connect(manageScoresButton, &FrogPilotButtonsControl::buttonClicked, [modelLayout, modelLabelsList, modelLabelsPanel, this](int id) { + FrogPilotButtonsControl *manageScoresBtn = new FrogPilotButtonsControl(title, desc, icon, {tr("RESET"), tr("VIEW")}); + QObject::connect(manageScoresBtn, &FrogPilotButtonsControl::buttonClicked, [this, modelLayout, modelLabelsList, modelLabelsPanel](int id) { if (id == 0) { - if (FrogPilotConfirmationDialog::yesorno(tr("Reset all model drives and ratings? This clears your drive history and collected feedback!"), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure you want to reset all of your model drives and scores?"), this)) { params.remove("ModelDrivesAndScores"); params_cache.remove("ModelDrivesAndScores"); } @@ -244,29 +304,92 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog modelLayout->setCurrentWidget(modelLabelsPanel); } }); - modelToggle = manageScoresButton; + modelToggle = manageScoresBtn; } else if (param == "SelectModel") { selectModelButton = new ButtonControl(title, tr("SELECT"), desc); QObject::connect(selectModelButton, &ButtonControl::clicked, [this]() { - QStringList selectableModels; + auto isInstalled = [this](const QString &key) { + bool has_thneed = false; + bool has_policy_meta = false; + bool has_policy_tg = false; + bool has_vision_meta = false; + bool has_vision_tg = false; + + for (const QString &file : modelDir.entryList(QDir::Files)) { + QFileInfo fi(modelDir.filePath(file)); + const QString base = fi.baseName(); + const QString ext = fi.suffix(); + if (!(base.startsWith(key) || base.startsWith(key + "_"))) continue; + + if (ext == "thneed") { + // Classic model (WD-40 etc.) + has_thneed = true; + } else if (ext == "pkl") { + // TinyGrad bundle uses these four exact suffixes + if (base.contains("_driving_policy_metadata")) has_policy_meta = true; + else if (base.contains("_driving_policy_tinygrad")) has_policy_tg = true; + else if (base.contains("_driving_vision_metadata")) has_vision_meta = true; + else if (base.contains("_driving_vision_tinygrad")) has_vision_tg = true; + } + } + + // Classic models: any matching .thneed counts as installed + if (has_thneed) return true; + // TinyGrad models: require all four policy/vision files to be present + return has_policy_meta && has_policy_tg && has_vision_meta && has_vision_tg; + }; + // Group models by series + QMap seriesToModels; for (const QString &modelKey : modelFileToNameMap.keys()) { QString modelName = modelFileToNameMap.value(modelKey); if (modelName.contains("(Default)")) { continue; } - if (modelDir.exists(modelKey + ".thneed") || hasAllTinygradFiles(modelDir, modelKey)) { - selectableModels.append(modelName); + if (isInstalled(modelKey)) { + QString series = modelSeriesMap.value(modelKey, "Dom Forgot To Label Me"); + seriesToModels[series].append(modelName); } } - selectableModels.sort(); - selectableModels.prepend(modelFileToNameMap.value(normalizeModelKey(QString::fromStdString(params_default.get("Model"))))); - QString modelToSelect = MultiOptionDialog::getSelection(tr("Select a Model β€” πŸ—ΊοΈ = Navigation | πŸ“‘ = Radar | πŸ‘€ = VOACC"), selectableModels, currentModel, this); + // Add Space Lab to Custom Series + QString spaceLabName = modelFileToNameMap.value("space-lab"); + if (isInstalled("space-lab")) { + seriesToModels["Custom Series"].append(spaceLabName); + } + + // Sort models within each series + for (QString &series : seriesToModels.keys()) { + seriesToModels[series].sort(); + } + + // Add default model to the beginning of its series + QString defaultModelName = modelFileToNameMap.value(QString::fromStdString(params_default.get("Model"))); + QString defaultSeries = modelSeriesMap.value(QString::fromStdString(params_default.get("Model")), "Custom Series"); + if (seriesToModels.contains(defaultSeries) && seriesToModels[defaultSeries].contains(defaultModelName)) { + seriesToModels[defaultSeries].removeAll(defaultModelName); + seriesToModels[defaultSeries].prepend(defaultModelName); + } + + QString modelToSelect = ExpandableMultiOptionDialog::getSelection(tr("Select a model - πŸ—ΊοΈ = Navigation | πŸ“‘ = Radar | πŸ‘€ = VOACC"), seriesToModels, currentModel, this); if (!modelToSelect.isEmpty()) { currentModel = modelToSelect; params.put("Model", modelFileToNameMap.key(modelToSelect).toStdString()); + // Sync ModelVersion with the selected model if known + { + QString modelKey = modelFileToNameMap.key(modelToSelect); + QFile vf("/data/models/.model_versions.json"); + if (vf.open(QIODevice::ReadOnly)) { + auto doc = QJsonDocument::fromJson(vf.readAll()); + if (doc.isObject()) { + auto obj = doc.object(); + if (obj.contains(modelKey)) { + params.put("ModelVersion", obj.value(modelKey).toString().toStdString()); + } + } + } + } updateFrogPilotToggles(); @@ -290,36 +413,16 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } } deletableModels.removeAll(processModelName(currentModel)); - deletableModels.removeAll(modelFileToNameMapProcessed.value(normalizeModelKey(QString::fromStdString(params_default.get("Model"))))); + deletableModels.removeAll(modelFileToNameMapProcessed.value(QString::fromStdString(params_default.get("Model")))); noModelsDownloaded = deletableModels.isEmpty(); } }); modelToggle = selectModelButton; - } else if (param == "UpdateTinygrad") { - updateTinygradButton = new FrogPilotButtonsControl(title, desc, icon, {tr("UPDATE")}); - QObject::connect(updateTinygradButton, &FrogPilotButtonsControl::buttonClicked, [this]() { - if (updatingTinygrad) { - params_memory.putBool("CancelModelDownload", true); - - updateTinygradButton->setEnabled(false); - updateTinygradButton->setValue(tr("Cancelling...")); - - cancellingDownload = true; - } else { - if (FrogPilotConfirmationDialog::yesorno(tr("Updating Tinygrad will delete existing Tinygrad-based driving models and need to be re-downloaded. Proceed?"), this)) { - params_memory.putBool("UpdateTinygrad", true); - params_memory.put("ModelDownloadProgress", "Downloading..."); - - updateTinygradButton->setText(0, tr("CANCEL")); - updateTinygradButton->setValue(tr("Updating...")); - - updatingTinygrad = true; - } - } - }); - modelToggle = updateTinygradButton; - + } else if (param == "StopDistance") { + std::vector stopDistanceButton{"Reset"}; + modelToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, 4.0, 10.0, QString(), std::map(), 0.1, false, {}, stopDistanceButton, false, false); + stopDistanceToggle = static_cast(modelToggle); } else { modelToggle = new ParamControl(param, title, desc, icon); } @@ -328,21 +431,16 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog modelList->addItem(modelToggle); - QObject::connect(modelToggle, &AbstractControl::hideDescriptionEvent, [this]() { - update(); - }); QObject::connect(modelToggle, &AbstractControl::showDescriptionEvent, [this]() { update(); }); } - openDescriptions(forceOpenDescriptions, toggles); - QObject::connect(static_cast(toggles["ModelRandomizer"]), &ToggleControl::toggleFlipped, [this](bool state) { updateToggles(); if (state && !allModelsDownloaded) { - if (FrogPilotConfirmationDialog::yesorno(tr("The \"Model Randomizer\" works only with downloaded models. Download all models now?"), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("The \"Model Randomizer\" only works with downloaded models. Do you want to download all the driving models?"), this)) { params_memory.putBool("DownloadAllModels", true); params_memory.put("ModelDownloadProgress", "Downloading..."); @@ -353,10 +451,17 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } }); - QObject::connect(parent, &FrogPilotSettingsWindow::closeSubPanel, [modelLayout, modelPanel, this] { - openDescriptions(forceOpenDescriptions, toggles); - modelLayout->setCurrentWidget(modelPanel); - }); + if (stopDistanceToggle) { + QObject::connect(stopDistanceToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this, stopDistanceToggle]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reset your Stop Distance to the default of 6 meters?"), tr("Reset"), this)) { + params.putFloat("StopDistance", 6.0); + stopDistanceToggle->refresh(); + updateFrogPilotToggles(); + } + }); + } + + QObject::connect(parent, &FrogPilotSettingsWindow::closeSubPanel, [modelLayout, modelPanel] {modelLayout->setCurrentWidget(modelPanel);}); QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotModelPanel::updateState); } @@ -368,28 +473,74 @@ void FrogPilotModelPanel::showEvent(QShowEvent *event) { tuningLevel = parent->tuningLevel; allModelsDownloading = params_memory.getBool("DownloadAllModels"); - modelDownloading = !params_memory.get("ModelDownloadProgress").empty(); - tinygradUpdate = params.getBool("TinygradUpdateAvailable"); - updatingTinygrad = params_memory.getBool("UpdateTinygrad"); - - modelDownloading &= !updatingTinygrad; + modelDownloading = !params_memory.get("ModelToDownload").empty(); QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(","); - availableModels.sort(); availableModelNames = QString::fromStdString(params.get("AvailableModelNames")).split(","); - availableModelNames.sort(); + availableModelSeries = QString::fromStdString(params.get("AvailableModelSeries")).split(","); + + // Build a simple model->version map for quick lookups elsewhere + { + QStringList versionList = QString::fromStdString(params.get("ModelVersions")).split(","); + QJsonObject versionObj; + int verCount = qMin(availableModels.size(), versionList.size()); + for (int i = 0; i < verCount; ++i) { + versionObj.insert(availableModels[i], versionList[i]); + } + QFile out("/data/models/.model_versions.json"); + if (out.open(QIODevice::WriteOnly)) { + out.write(QJsonDocument(versionObj).toJson()); + out.close(); + } + } modelFileToNameMap.clear(); modelFileToNameMapProcessed.clear(); - for (int i = 0; i < qMin(availableModels.size(), availableModelNames.size()); ++i) { + modelSeriesMap.clear(); + int size = qMin(qMin(availableModels.size(), availableModelNames.size()), availableModelSeries.size()); + for (int i = 0; i < size; ++i) { modelFileToNameMap.insert(availableModels[i], availableModelNames[i]); modelFileToNameMapProcessed.insert(availableModels[i], processModelName(availableModelNames[i])); + modelSeriesMap.insert(availableModels[i], availableModelSeries[i]); } + modelFileToNameMap.insert("space-lab", "Space Lab πŸ‘€πŸ“‘"); + modelFileToNameMapProcessed.insert("space-lab", "Space Lab"); + modelSeriesMap.insert("space-lab", "Dom Forgot To Label Me"); + auto isInstalled = [this](const QString &key) { + bool has_thneed = false; + bool has_policy_meta = false; + bool has_policy_tg = false; + bool has_vision_meta = false; + bool has_vision_tg = false; + + for (const QString &file : modelDir.entryList(QDir::Files)) { + QFileInfo fi(modelDir.filePath(file)); + const QString base = fi.baseName(); + const QString ext = fi.suffix(); + if (!(base.startsWith(key) || base.startsWith(key + "_"))) continue; + + if (ext == "thneed") { + // Classic model (WD-40 etc.) + has_thneed = true; + } else if (ext == "pkl") { + // TinyGrad bundle uses these four exact suffixes + if (base.contains("_driving_policy_metadata")) has_policy_meta = true; + else if (base.contains("_driving_policy_tinygrad")) has_policy_tg = true; + else if (base.contains("_driving_vision_metadata")) has_vision_meta = true; + else if (base.contains("_driving_vision_tinygrad")) has_vision_tg = true; + } + } + + // Classic models: any matching .thneed counts as installed + if (has_thneed) return true; + // TinyGrad models: require all four policy/vision files to be present + return has_policy_meta && has_policy_tg && has_vision_meta && has_vision_tg; + }; QStringList downloadableModels = availableModelNames; for (const QString &modelKey : modelFileToNameMap.keys()) { QString modelName = modelFileToNameMap.value(modelKey); - if (modelDir.exists(modelKey + ".thneed") || hasAllTinygradFiles(modelDir, modelKey)) { + if (isInstalled(modelKey)) { downloadableModels.removeAll(modelName); } } @@ -408,12 +559,12 @@ void FrogPilotModelPanel::showEvent(QShowEvent *event) { } } deletableModels.removeAll(processModelName(currentModel)); - deletableModels.removeAll(modelFileToNameMapProcessed.value(normalizeModelKey(QString::fromStdString(params_default.get("Model"))))); + deletableModels.removeAll(modelFileToNameMapProcessed.value(QString::fromStdString(params_default.get("Model")))); noModelsDownloaded = deletableModels.isEmpty(); - QString modelKey = normalizeModelKey(QString::fromStdString(params.get("Model"))); - if (!modelDir.exists(modelKey + ".thneed") && !hasAllTinygradFiles(modelDir, modelKey)) { - modelKey = normalizeModelKey(QString::fromStdString(params_default.get("Model"))); + QString modelKey = QString::fromStdString(params.get("Model")); + if (!isInstalled(modelKey)) { + modelKey = QString::fromStdString(params_default.get("Model")); } currentModel = modelFileToNameMap.value(modelKey); selectModelButton->setValue(currentModel); @@ -422,14 +573,11 @@ void FrogPilotModelPanel::showEvent(QShowEvent *event) { deleteModelButton->setEnabled(!(allModelsDownloading || modelDownloading || noModelsDownloaded)); - downloadModelButton->setEnabledButtons(0, !allModelsDownloaded && !allModelsDownloading && !cancellingDownload && !updatingTinygrad && fs.frogpilot_scene.online && parked); - downloadModelButton->setEnabledButtons(1, !allModelsDownloaded && !modelDownloading && !cancellingDownload && !updatingTinygrad && fs.frogpilot_scene.online && parked); + downloadModelButton->setEnabledButtons(0, !allModelsDownloaded && !allModelsDownloading && !cancellingDownload && fs.frogpilot_scene.online && parked); + downloadModelButton->setEnabledButtons(1, !allModelsDownloaded && !modelDownloading && !cancellingDownload && fs.frogpilot_scene.online && parked); downloadModelButton->setValue(fs.frogpilot_scene.online ? (parked ? "" : "Not parked") : tr("Offline...")); - updateTinygradButton->setEnabled(!modelDownloading && !cancellingDownload && fs.frogpilot_scene.online && parked && tinygradUpdate); - updateTinygradButton->setValue(tinygradUpdate ? tr("Update available!") : tr("Up to date!")); - started = s.scene.started; updateToggles(); @@ -444,32 +592,27 @@ void FrogPilotModelPanel::updateState(const UIState &s, const FrogPilotUIState & if (allModelsDownloading || modelDownloading) { QString progress = QString::fromStdString(params_memory.get("ModelDownloadProgress")); - bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|missing|offline", QRegularExpression::CaseInsensitiveOption)); + bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|offline", QRegularExpression::CaseInsensitiveOption)); if (progress != "Downloading...") { downloadModelButton->setValue(progress); } - if (progress == "All models downloaded!" || progress == "Downloaded!" && !allModelsDownloading || downloadFailed) { + if (progress == "All models downloaded!" && allModelsDownloading || progress == "Downloaded!" && modelDownloading || downloadFailed) { finalizingDownload = true; - QTimer::singleShot(2500, [progress, this]() { + QTimer::singleShot(2500, [this, progress]() { + allModelsDownloaded = progress == "All models downloaded!"; allModelsDownloading = false; cancellingDownload = false; finalizingDownload = false; modelDownloading = false; noModelsDownloaded = false; - QStringList downloadableModels = availableModelNames; - for (const QString &modelKey : modelFileToNameMap.keys()) { - QString modelName = modelFileToNameMap.value(modelKey); - if (modelDir.exists(modelKey + ".thneed") || hasAllTinygradFiles(modelDir, modelKey)) { - downloadableModels.removeAll(modelName); - } - } - allModelsDownloaded = downloadableModels.isEmpty(); - + params_memory.remove("CancelModelDownload"); + params_memory.remove("DownloadAllModels"); params_memory.remove("ModelDownloadProgress"); + params_memory.remove("ModelToDownload"); downloadModelButton->setEnabled(true); downloadModelButton->setValue(""); @@ -479,58 +622,20 @@ void FrogPilotModelPanel::updateState(const UIState &s, const FrogPilotUIState & downloadModelButton->setValue(fs.frogpilot_scene.online ? (parked ? "" : "Not parked") : tr("Offline...")); } - if (updatingTinygrad) { - QString progress = QString::fromStdString(params_memory.get("ModelDownloadProgress")); - bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|missing|offline", QRegularExpression::CaseInsensitiveOption)); - - if (progress != "Downloading...") { - updateTinygradButton->setValue(progress); - } - - if (progress == "Updated!" && updatingTinygrad || downloadFailed) { - finalizingDownload = true; - - QTimer::singleShot(2500, [progress, this]() { - modelDownloading = !params_memory.get("ModelDownloadProgress").empty(); - - if (modelDownloading) { - downloadModelButton->setText(1, tr("CANCEL")); - - downloadModelButton->setValue("Downloading..."); - - downloadModelButton->setVisibleButton(0, false); - } else { - cancellingDownload = false; - } - - tinygradUpdate = params.getBool("TinygradUpdateAvailable"); - - finalizingDownload = false; - updatingTinygrad = false; - - updateTinygradButton->setEnabled(tinygradUpdate); - updateTinygradButton->setText(0, tr("UPDATE")); - updateTinygradButton->setValue(tinygradUpdate ? tr("Update available!") : tr("Up to date!")); - }); - } - } - deleteModelButton->setEnabled(!(allModelsDownloading || modelDownloading || noModelsDownloaded)); downloadModelButton->setText(0, modelDownloading ? tr("CANCEL") : tr("DOWNLOAD")); downloadModelButton->setText(1, allModelsDownloading ? tr("CANCEL") : tr("DOWNLOAD ALL")); - downloadModelButton->setEnabledButtons(0, !allModelsDownloaded && !allModelsDownloading && !cancellingDownload && !finalizingDownload && !updatingTinygrad && fs.frogpilot_scene.online && parked); - downloadModelButton->setEnabledButtons(1, !allModelsDownloaded && !modelDownloading && !cancellingDownload && !finalizingDownload && !updatingTinygrad && fs.frogpilot_scene.online && parked); + downloadModelButton->setEnabledButtons(0, !allModelsDownloaded && !allModelsDownloading && !cancellingDownload && fs.frogpilot_scene.online && parked); + downloadModelButton->setEnabledButtons(1, !allModelsDownloaded && !modelDownloading && !cancellingDownload && fs.frogpilot_scene.online && parked); downloadModelButton->setVisibleButton(0, !allModelsDownloading); downloadModelButton->setVisibleButton(1, !modelDownloading); - updateTinygradButton->setEnabled(!modelDownloading && !cancellingDownload && !cancellingDownload && !finalizingDownload && fs.frogpilot_scene.online && parked && tinygradUpdate); - started = s.scene.started; - parent->keepScreenOn = allModelsDownloading || modelDownloading || updatingTinygrad; + parent->keepScreenOn = allModelsDownloading || modelDownloading; } void FrogPilotModelPanel::updateModelLabels(FrogPilotListWidget *labelsList) { @@ -565,12 +670,12 @@ void FrogPilotModelPanel::updateToggles() { else if (key == "SelectModel") { setVisible &= !params.getBool("ModelRandomizer"); + } else if (key == "StopDistance") { + setVisible &= (tuningLevel == 3); // Only visible in developer tuning level } toggle->setVisible(setVisible); } - openDescriptions(forceOpenDescriptions, toggles); - update(); } diff --git a/frogpilot/ui/qt/offroad/model_settings.h b/frogpilot/ui/qt/offroad/model_settings.h index 80b2c639c..e933d4f49 100644 --- a/frogpilot/ui/qt/offroad/model_settings.h +++ b/frogpilot/ui/qt/offroad/model_settings.h @@ -55,8 +55,10 @@ private: QMap modelFileToNameMap; QMap modelFileToNameMapProcessed; + QMap modelSeriesMap; QString currentModel; QStringList availableModelNames; + QStringList availableModelSeries; }; diff --git a/frogpilot/ui/qt/offroad/visual_settings.cc b/frogpilot/ui/qt/offroad/visual_settings.cc index 7f7be8e8f..5624a368b 100644 --- a/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/frogpilot/ui/qt/offroad/visual_settings.cc @@ -214,6 +214,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(FrogPilotSettingsWindow *parent) : {13, tr("Longitudinal MPC Jerk: Acceleration")}, {14, tr("Longitudinal MPC Jerk: Danger Zone")}, {15, tr("Longitudinal MPC Jerk: Speed Control")}, + {16, tr("Driving Model: Current")}, }; ButtonControl *metricToggle = new ButtonControl(title, tr("SELECT"), desc); diff --git a/launch_env.sh b/launch_env.sh index 87f3376a7..aed382da1 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="10.1" + export AGNOS_VERSION="10.1.1" fi export STAGING_ROOT="/data/safe_staging" diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 1c78dd7b0..3cbb2d756 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -82,6 +82,12 @@ VAL_TABLE_ HandsOffSWDetectionMode 2 "Failed" 1 "Enabled" 0 "Disabled" ; BO_ 189 EBCMRegenPaddle: 7 K17_EBCM SG_ RegenPaddle : 7|4@0+ (1,0) [0|0] "" NEO + SG_ Byte1 : 8|8@1+ (1,0) [0|255] "" NEO + SG_ Byte2 : 16|8@1+ (1,0) [0|255] "" NEO + SG_ Byte3 : 24|8@1+ (1,0) [0|255] "" NEO + SG_ Byte4 : 32|8@1+ (1,0) [0|255] "" NEO + SG_ Byte5 : 40|8@1+ (1,0) [0|255] "" NEO + SG_ Byte6 : 48|8@1+ (1,0) [0|255] "" NEO BO_ 190 ECMAcceleratorPos: 6 K20_ECM SG_ BrakePedalPos : 15|8@0+ (1,0) [0|0] "sticky" NEO @@ -192,10 +198,15 @@ BO_ 500 SportMode: 6 XXX SG_ SportMode : 15|1@0+ (1,0) [0|1] "" XXX BO_ 501 ECMPRDNL2: 8 K20_ECM - SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO + SG_ Byte0 : 0|8@1+ (1,0) [0|255] "" NEO + SG_ Byte1 : 8|8@1+ (1,0) [0|255] "" NEO + SG_ Byte2 : 16|8@1+ (1,0) [0|255] "" NEO SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO + SG_ Byte4 : 32|8@1+ (1,0) [0|255] "" NEO SG_ ManualMode : 41|1@0+ (1,0) [0|1] "" NEO - + SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO + SG_ Byte7 : 56|8@1+ (1,0) [0|255] "" NEO + BO_ 532 BRAKE_RELATED: 6 XXX SG_ UserBrakePressure : 0|9@0+ (1,0) [0|511] "" XXX @@ -370,6 +381,6 @@ VAL_ 715 GasRegenCmdActive 1 "Active" 0 "Inactive" ; VAL_ 320 Intellibeam 1 "Active" 0 "Inactive" ; VAL_ 320 HighBeamsActive 1 "Active" 0 "Inactive" ; VAL_ 320 HighBeamsTemporary 1 "Active" 0 "Inactive" ; -VAL_ 501 PRNDL2 6 "L" 4 "D" 3 "N" 2 "R" 1 "P" 0 "Shifting"; +VAL_ 501 PRNDL2 7 "L2" 6 "L" 5 "L3" 4 "D" 3 "N" 2 "R" 1 "P" 0 "Shifting"; VAL_ 501 TransmissionState 11 "Shifting" 10 "Reverse" 9 "Forward" 8 "Disengaged"; VAL_ 501 ManualMode 1 "Active" 0 "Inactive" diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index 55d9985af..69e23a86d 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -202,9 +202,9 @@ void ignition_can_hook(CANPacket_t *to_push) { int len = GET_LEN(to_push); // GM exception - if ((addr == 0x1F1) && (len == 8)) { - // SystemPowerMode (2=Run, 3=Crank Request) - ignition_can = (GET_BYTE(to_push, 0) & 0x2U) != 0U; + if ((addr == 0xC9) && (len == 8)) { + // Matches SystemPowerMode (1=Run, 0=Off) + ignition_can = (GET_BYTE(to_push, 6) & 0x10U) != 0U; ignition_can_cnt = 0U; } diff --git a/panda/board/safety.h b/panda/board/safety.h index 9c16bd434..7eb266127 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -88,7 +88,7 @@ int safety_fwd_hook(int bus_num, int addr) { } bool get_longitudinal_allowed(void) { - return controls_allowed && !gas_pressed_prev; + return controls_allowed && !gas_pressed; } // Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8 diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 9d2ad9346..dfa936e69 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -29,23 +29,23 @@ const int GM_STANDSTILL_THRSLD = 10; // 0.311kph // panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches // If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state -const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (675 + 355) / 2 ratio between offset and gain from dbc file +const int GM_GAS_INTERCEPTOR_THRESHOLD = 595; // (675 + 355) / 2 ratio between offset and gain from dbc file #define GM_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks -const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus +const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus {0x315, 2, 5}}; // ch bus -const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, // pt bus +const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus -const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus +const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus -const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus +const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x184, 2, 8}}; // camera bus -const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus +const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, {0xBD, 0, 7}, {0x1F5, 0, 8}, // pt bus {0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now. @@ -172,6 +172,13 @@ static void gm_rx_hook(const CANPacket_t *to_push) { } } + // Cruise check for ACC models with pedal interceptor - block stock ACC + if ((addr == 0x1C4) && gm_has_acc && enable_gas_interceptor) { + // When pedal interceptor is active on ACC models, ignore stock cruise state + // to prevent conflicts between pedal interceptor and stock ACC + cruise_engaged_prev = false; + } + if (addr == 0xBD) { regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U; } @@ -192,6 +199,12 @@ static void gm_rx_hook(const CANPacket_t *to_push) { } generic_rx_checks(stock_ecu_detected); } + // Cruise check for Gen2 Bolt (ASCMActiveCruiseControlStatus on bus 2) + int addr = GET_ADDR(to_push); + if ((addr == 0x370) && (GET_BUS(to_push) == 2U)) { + bool cruise_engaged = (GET_BYTE(to_push, 2) >> 7) != 0U; // ACCCmdActive + cruise_engaged_prev = cruise_engaged; + } } static bool gm_tx_hook(const CANPacket_t *to_send) { @@ -232,7 +245,7 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { int gas_regen = ((GET_BYTE(to_send, 2) & 0x7FU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3); bool violation = false; - // Allow apply bit in pre-enabled and overriding states + // Allow apply bit in pre-enabled and overriding states, except for inactive gas // Allow apply bit in pre-enabled and overriding states violation |= !controls_allowed && apply; violation |= longitudinal_gas_checks(gas_regen, *gm_long_limits); @@ -246,6 +259,11 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U; bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev; + // For ACC cars with pedal interceptor, allow cancel even if cruise_engaged_prev is false + // (since we set it to false to prevent conflicts, but still need to cancel cruise) + if (gm_hw == GM_CAM && enable_gas_interceptor && button == GM_BTN_CANCEL) { + allowed_btn = true; + } // For standard CC, allow spamming of SET / RESUME if (gm_cc_long) { allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS); @@ -256,6 +274,22 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { } } + // REGEN PADDLE + if (addr == 0xBD) { + bool regen_apply = GET_BIT(to_send, 7) || GET_BIT(to_send, 6) || GET_BIT(to_send, 5) || GET_BIT(to_send, 4); + if (!controls_allowed && regen_apply) { + tx = false; + } + } + + // PRNDL2 regen check (7 for Gen0, Gen1. 5 For Gen2) + if (addr == 0x1F5) { + uint8_t prndl2 = GET_BYTE(to_send, 3) & 0xF; + bool prndl_apply = (prndl2 == 7) || (prndl2 == 5); + if (!controls_allowed && prndl_apply) { + tx = false; + } + } return tx; } @@ -272,9 +306,13 @@ static int gm_fwd_hook(int bus_num, int addr) { } if (bus_num == 2) { - // block lkas message and acc messages if gm_cam_long, forward all others + // block lkas message and acc messages + // Block 0x370 only for experimental long without pedal interceptor bool is_lkas_msg = (addr == 0x180); - bool is_acc_msg = (addr == 0x315) || (addr == 0x2CB) || (addr == 0x370); + bool is_acc_msg = (addr == 0x315) || (addr == 0x2CB); + if (gm_cam_long && !enable_gas_interceptor) { + is_acc_msg = is_acc_msg || (addr == 0x370); + } bool block_msg = is_lkas_msg || (is_acc_msg && gm_cam_long); if (!block_msg) { bus_fwd = 0; @@ -306,6 +344,10 @@ static safety_config gm_init(uint16_t param) { gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG); gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG); gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG) && !gm_cc_long; + // Block ACC messages when pedal interceptor is active on ACC models + if (gm_hw == GM_CAM && enable_gas_interceptor) { + gm_cam_long = true; + } gm_pcm_cruise = ((gm_hw == GM_CAM) && (!gm_cam_long || gm_cc_long) && !gm_force_ascm && !gm_pedal_long) || (gm_hw == GM_SDGM); gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA); gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC); diff --git a/panda/tests/safety/common.py b/panda/tests/safety/common.py index fc746f75f..19074d623 100644 --- a/panda/tests/safety/common.py +++ b/panda/tests/safety/common.py @@ -922,7 +922,7 @@ class PandaSafetyTest(PandaSafetyTestBase): continue if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}): continue - if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmSdgmSafety', 'TestGmInterceptorSafety', 'TestGmCcLongitudinalSafety'}): + if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmSdgmSafety', 'TestGmInterceptorSafety', 'TestGmCcLongitudinalSafety', 'TestGmAscmSafety'}): continue if attr.startswith('TestFord') and current_test.startswith('TestFord'): continue diff --git a/panda/tests/safety/test.sh b/panda/tests/safety/test.sh old mode 100644 new mode 100755 diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index 4c140f915..2235d4674 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -148,16 +148,17 @@ class TestGmSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSaf class TestGmAscmSafety(GmLongitudinalBase, TestGmSafetyBase): - TX_MSGS = [[0x180, 0], [0x409, 0], [0x40A, 0], [0x2CB, 0], [0x370, 0], # pt bus + TX_MSGS = [[0x180, 0], [0x409, 0], [0x40A, 0], [0x2CB, 0], [0x370, 0], [0x1F5, 0], # pt bus [0xA1, 1], [0x306, 1], [0x308, 1], [0x310, 1], # obs bus [0x315, 2]] # ch bus FWD_BLACKLISTED_ADDRS: dict[int, list[int]] = {} FWD_BUS_LOOKUP: dict[int, int] = {} BRAKE_BUS = 2 - MAX_GAS = 3072 - MIN_GAS = 1404 # maximum regen - INACTIVE_GAS = 1404 + MAX_GAS = 7168 + MIN_GAS = 5500 # maximum regen + INACTIVE_GAS = 5500 + MAX_POSSIBLE_GAS = 8192 def setUp(self): self.packer = CANPackerPanda("gm_global_a_powertrain_generated") @@ -166,6 +167,22 @@ class TestGmAscmSafety(GmLongitudinalBase, TestGmSafetyBase): self.safety.set_safety_hooks(Panda.SAFETY_GM, 0) self.safety.init_tests() + def test_regen_paddle(self): + # Regen paddle should only be allowed when controls are allowed and regen is applied + regen_values = {"RegenPaddle": 16} # Set bit 4 for regen apply + regen_msg = self.packer.make_can_msg_panda("EBCMRegenPaddle", 0, regen_values) + + prndl_values = {"PRNDL2": 7, "ManualMode": 1} # Transmission message + prndl_msg = self.packer.make_can_msg_panda("ECMPRDNL2", 0, prndl_values) + + self.safety.set_controls_allowed(0) + self.assertTrue(self._tx(regen_msg)) + self.assertFalse(self._tx(prndl_msg)) + + self.safety.set_controls_allowed(1) + self.assertTrue(self._tx(regen_msg)) + self.assertTrue(self._tx(prndl_msg)) + class TestGmCameraSafetyBase(TestGmSafetyBase): @@ -184,7 +201,7 @@ class TestGmCameraSafetyBase(TestGmSafetyBase): class TestGmCameraSafety(TestGmCameraSafetyBase): - TX_MSGS = [[0x180, 0], # pt bus + TX_MSGS = [[0x180, 0], [0x1F5, 0], # pt bus [0x184, 2]] # camera bus FWD_BLACKLISTED_ADDRS = {2: [0x180], 0: [0x184]} # block LKAS message and PSCMStatus BUTTONS_BUS = 2 # tx only @@ -203,6 +220,7 @@ class TestGmCameraSafety(TestGmCameraSafetyBase): self.assertFalse(self._tx(self._button_msg(btn))) self.safety.set_controls_allowed(1) + self._rx(self._pcm_status_msg(False)) for btn in range(8): self.assertFalse(self._tx(self._button_msg(btn))) @@ -211,15 +229,17 @@ class TestGmCameraSafety(TestGmCameraSafetyBase): self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL))) + class TestGmCameraLongitudinalSafety(GmLongitudinalBase, TestGmCameraSafetyBase): - TX_MSGS = [[0x180, 0], [0x315, 0], [0x2CB, 0], [0x370, 0], # pt bus + TX_MSGS = [[0x180, 0], [0x315, 0], [0x2CB, 0], [0x370, 0], [0x1F5, 0], # pt bus [0x184, 2]] # camera bus FWD_BLACKLISTED_ADDRS = {2: [0x180, 0x2CB, 0x370, 0x315], 0: [0x184]} # block LKAS, ACC messages and PSCMStatus BUTTONS_BUS = 0 # rx only - MAX_GAS = 3400 - MIN_GAS = 1514 # maximum regen - INACTIVE_GAS = 1554 + MAX_GAS = 7496 + MIN_GAS = 5610 # maximum regen + INACTIVE_GAS = 5650 + MAX_POSSIBLE_GAS = 8192 def setUp(self): self.packer = CANPackerPanda("gm_global_a_powertrain_generated") @@ -228,10 +248,26 @@ class TestGmCameraLongitudinalSafety(GmLongitudinalBase, TestGmCameraSafetyBase) self.safety.set_safety_hooks(Panda.SAFETY_GM, Panda.FLAG_GM_HW_CAM | Panda.FLAG_GM_HW_CAM_LONG) self.safety.init_tests() + def test_regen_paddle(self): + # Regen paddle should only be allowed when controls are allowed and regen is applied + regen_values = {"RegenPaddle": 16} # Set bit 4 for regen apply + regen_msg = self.packer.make_can_msg_panda("EBCMRegenPaddle", 0, regen_values) + + prndl_values = {"PRNDL2": 7, "ManualMode": 1} # Transmission message + prndl_msg = self.packer.make_can_msg_panda("ECMPRDNL2", 0, prndl_values) + + self.safety.set_controls_allowed(0) + self.assertTrue(self._tx(regen_msg)) + self.assertFalse(self._tx(prndl_msg)) + + self.safety.set_controls_allowed(1) + self.assertTrue(self._tx(regen_msg)) + self.assertTrue(self._tx(prndl_msg)) + class TestGmSdgmSafety(TestGmSafetyBase): FWD_BUS_LOOKUP = {0: 2, 2: 0} - TX_MSGS = [[0x180, 0], [0x1E1, 0], # pt bus - [0x184, 2]] # obj bus + TX_MSGS = [[0x180, 0], [0x1E1, 0], [0x1F5, 0], # pt bus + [0x184, 2]] # obj bus FWD_BLACKLISTED_ADDRS = {2: [0x180], 0: [0x184]} # block LKAS message and PSCMStatus BUTTONS_BUS = 0 # tx @@ -272,7 +308,7 @@ def interceptor_msg(gas, addr): class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafety): - INTERCEPTOR_THRESHOLD = 515 + INTERCEPTOR_THRESHOLD = 595 def setUp(self): self.packer = CANPackerPanda("gm_global_a_powertrain_generated") @@ -313,16 +349,20 @@ class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafet # Only CANCEL button is allowed while cruise is enabled self.safety.set_controls_allowed(0) for btn in range(8): - self.assertFalse(self._tx(self._button_msg(btn))) + expected = (btn == Buttons.CANCEL) + self.assertEqual(expected, self._tx(self._button_msg(btn))) self.safety.set_controls_allowed(1) for btn in range(8): - self.assertFalse(self._tx(self._button_msg(btn))) + # For GM interceptor with CAM hardware, CANCEL is always allowed + expected = (btn == Buttons.CANCEL) + self.assertEqual(expected, self._tx(self._button_msg(btn))) self.safety.set_controls_allowed(1) for enabled in (True, False): self._rx(self._pcm_status_msg(enabled)) - self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL))) + # For GM CAM with gas interceptor, CANCEL is always allowed + self.assertTrue(self._tx(self._button_msg(Buttons.CANCEL))) self.assertTrue(self.safety.get_controls_allowed()) def test_fwd_hook(self): @@ -346,7 +386,7 @@ class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafet class TestGmCcLongitudinalSafety(TestGmCameraSafety): - TX_MSGS = [[384, 0], [481, 0], [388, 2]] + TX_MSGS = [[384, 0], [481, 0], [0x1F5, 0], [388, 2]] FWD_BLACKLISTED_ADDRS = {2: [384], 0: [388]} # block LKAS message and PSCMStatus BUTTONS_BUS = 0 # tx only @@ -384,5 +424,12 @@ class TestGmCcLongitudinalSafety(TestGmCameraSafety): self.assertEqual(enabled, self._tx(self._button_msg(btn))) + # FrogPilot tests + def _toggle_aol(self, toggle_on): + # ECMEngineStatus, bit 29 is CruiseMainOn + values = {"CruiseMainOn": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("ECMEngineStatus", 0, values) + + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png index b9b4ed97d..0d735edaa 100644 Binary files a/selfdrive/assets/img_spinner_comma.png and b/selfdrive/assets/img_spinner_comma.png differ diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 664214bc8..ef9a32789 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,3 +1,5 @@ +from typing import Tuple +import time from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter @@ -7,10 +9,11 @@ from openpilot.common.params_pyx import Params from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command from openpilot.selfdrive.car.gm import gmcan -from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR +from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, CC_REGEN_PADDLE_CAR from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.swaglog import cloudlog VisualAlert = car.CarControl.HUDControl.VisualAlert NetworkLocation = car.CarParams.NetworkLocation @@ -22,7 +25,15 @@ TransmissionType = car.CarParams.TransmissionType CAMERA_CANCEL_DELAY_FRAMES = 10 # Enforce a minimum interval between steering messages to avoid a fault MIN_STEER_MSG_INTERVAL_MS = 15 - +# Two‑sided spacing tuned for ~33 Hz steer; target a 10 ms wide window per interval +# Paddle spoofing and scheduling constants +PADDLE_STEER_GAP_MIN_NS = 5_000_000 # β‰₯5 ms each side (EPS guard) +PADDLE_STEER_GAP_MAX_NS = 12_000_000 # cap for long intervals +PADDLE_GAP_TARGET_NS = 5_000_000 # aim per‑side gap even if interval//2 βˆ’ early is larger +PADDLE_NONBLOCK_GAP_NS = 1_000_000 # β‰₯1 ms since last paddle send +PADDLE_SLOT_EARLY_NS = 1_000_000 # allow firing up to 1 ms before slot +OVERFLOW_THRESH = 1.00 # fire one extra slot whenever credits β‰₯ 1.0 +PADDLE_TARGET_HZ = 42.0 # desired paddle rate (Hz) when regen active; steer is ~33 Hz # Constants for pitch compensation BRAKE_PITCH_FACTOR_BP = [5., 10.] # [m/s] smoothly revert to planned accel at low speeds BRAKE_PITCH_FACTOR_V = [0., 1.] # [unitless in [0,1]]; don't touch @@ -38,6 +49,11 @@ class CarController(CarControllerBase): self.apply_speed = 0 self.frame = 0 self.last_steer_frame = 0 + self.last_steer_ts_ns = 0 + self.last_regen_active = False + self.prev_steer_ts_ns = 0 + self.last_spoof_ts_ns = 0 + self.last_paddle_ts_ns = 0 self.last_button_frame = 0 self.cancel_counter = 0 self.pedal_steady = 0. @@ -56,25 +72,69 @@ class CarController(CarControllerBase): self.accel_g = 0.0 self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz + self.accel_g = 0.0 + self.regen_paddle_pressed = False + self.aego = 0.0 + self.regen_paddle_timer = 0 - @staticmethod - def calc_pedal_command(accel: float, long_active: bool) -> float: - if not long_active: return 0. - zero = 0.15625 # 40/256 - if accel > 0.: - # Scales the accel from 0-1 to 0.156-1 - pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.) - else: - # if accel is negative, -0.1 -> 0.015625 - pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen - return pedal_gas + # Midpoint + overflow spoof accumulator and flags + self.spoof_accum = 0.0 + self.spoof_mid_sent = False + self.spoof_over_sent = False + self.last_interval_ns = 0 + + def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]: + if not long_active: + return 0., False + + # Regen paddle hysteresis (frame-based): hold 10 frames, with decrement dead-zone + if not hasattr(self, 'regen_paddle_timer'): + self.regen_paddle_timer = 0 # frames + + # Regen paddle hysteresis (frame‑based): count frames when decelerating hard, decrement only when truly released + if self.aego < -0.7: + self.regen_paddle_timer += 1 + elif self.aego > -0.3: + self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0) + # else: hold timer between -0.7 and -0.3 + + # Base paddle press hysteresis + self.regen_paddle_pressed = self.regen_paddle_timer >= 10 # 10 frames + press_regen_paddle = self.regen_paddle_pressed + + + # Regen gain ratios from bin-averaged 60–0 deceleration sweep; Calculates stronger decel from paddle + speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63, + 11.749, 12.868, 13.987, 15.106, 16.225, 17.344, 18.463, 19.582, 20.701, 21.820, + 22.939, 24.058, 25.177, 26.296] + regen_gain_ratio = [ + 1.000000, 1.057308, 1.131123, 1.220611, 1.270247, 1.300253, 1.339543, 1.361002, + 1.388410, 1.403253, 1.414721, 1.430949, 1.420289, 1.436787, 1.434116, 1.436805, + 1.417508, 1.402213, 1.395360, 1.360921, 1.342030, 1.292219, 1.270048, 1.239172 + ] + + gain = interp(car_velocity, speed_mps, regen_gain_ratio) + pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240]) + + # Compute raw pedal gas + raw_pedal_gas = clip((pedaloffset + (accel / gain) * 0.6), 0.0, 1.0) if press_regen_paddle else clip((pedaloffset + accel * 0.6), 0.0, 1.0) + + # --- Immediate application of raw pedal gas, no blending --- + pedal_gas = raw_pedal_gas + # Safety cap: ramp from 22% at 0 m/s to 37.25% at 10 mph (4.47 m/s), then allow full throttle + pedal_gas_max = interp(car_velocity, [0.0, 4.47, 4.48], [0.22, 0.3725, 1.0]) + pedal_gas = clip(pedal_gas, 0.0, pedal_gas_max) + return pedal_gas, press_regen_paddle def update(self, CC, CS, now_nanos, frogpilot_toggles): + self.CS = CS + self.aego = CS.out.aEgo actuators = CC.actuators accel = brake_accel = actuators.accel + press_regen_paddle = False hud_control = CC.hudControl hud_alert = hud_control.visualAlert hud_v_cruise = hud_control.setSpeed @@ -83,6 +143,138 @@ class CarController(CarControllerBase): # Send CAN commands. can_sends = [] + paddle_sends = [] + + raw_regen_active = ( + self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and + self.CP.openpilotLongitudinalControl and + CC.longActive and + self.CP.enableGasInterceptor and + self.regen_paddle_timer >= 10 # raw hysteresis-only (10 frames) + ) + regen_active = raw_regen_active + + # === Spoof scheduling: midpoint + overflow (~target Hz) === + # Rising-edge reset on regen start + if raw_regen_active and not self.last_regen_active: + self.prev_steer_ts_ns = self.last_steer_ts_ns + self.last_spoof_ts_ns = 0 + self.spoof_accum = 0.0 + self.spoof_mid_sent = False + self.spoof_over_sent = False + + if raw_regen_active: + # Interval between last two bus-0 steer sends + interval_ns = self.last_steer_ts_ns - self.prev_steer_ts_ns + + # Adaptive two‑sided gap sized to the current steer interval, but capped to a target so the window stays wide enough + gap_ns = (PADDLE_STEER_GAP_MIN_NS if interval_ns <= 0 else + max(PADDLE_STEER_GAP_MIN_NS, + min(PADDLE_STEER_GAP_MAX_NS, + min((interval_ns // 2) - PADDLE_SLOT_EARLY_NS, PADDLE_GAP_TARGET_NS)))) + + # New steer interval? clear per-interval flags and add credits to reach target Hz + if interval_ns != self.last_interval_ns: + self.spoof_mid_sent = False + self.spoof_over_sent = False + self.last_interval_ns = interval_ns + # Add credits once per new steer interval to reach the desired paddle rate + if interval_ns > 0: + steer_hz = 1e9 / float(interval_ns) + extra_needed = max(0.0, (PADDLE_TARGET_HZ / steer_hz) - 1.0) # e.g., 42/33 βˆ’ 1 β‰ˆ 0.2727 + self.spoof_accum += extra_needed + + # Midpoint spoof: one per interval + if not self.spoof_mid_sent and interval_ns > 0: + midpoint_ns = self.prev_steer_ts_ns + interval_ns // 2 + cloudlog.error("PADDLE MID: Ξ”after=%.1fms Ξ”before=%.1fms credits=%.3f timer=%d", + (now_nanos - self.last_steer_ts_ns) * 1e-6, + (now_nanos - self.prev_steer_ts_ns) * 1e-6, + self.spoof_accum, + self.regen_paddle_timer) + # Compute spacing to last and next steer (two-sided guard) + next_steer_ts_ns = self.last_steer_ts_ns + interval_ns if interval_ns > 0 else 0 + delta_after_ns = now_nanos - self.last_steer_ts_ns + delta_before_ns = (next_steer_ts_ns - now_nanos) if interval_ns > 0 else 1_000_000_000 + if (CS.out.vEgo > 2.68 + and now_nanos >= (midpoint_ns - PADDLE_SLOT_EARLY_NS) + and delta_after_ns >= gap_ns + and delta_before_ns >= gap_ns): + # Non-blocking 1 ms spacing for paddle frames + if now_nanos - self.last_paddle_ts_ns >= PADDLE_NONBLOCK_GAP_NS: + paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True)) + paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True)) + self.last_paddle_ts_ns = now_nanos + self.last_spoof_ts_ns = now_nanos + self.spoof_mid_sent = True + + # Overflow spoof: insert extra when accumulator allows + if self.spoof_accum >= OVERFLOW_THRESH and not self.spoof_over_sent and interval_ns > 0: + slot2_ns = self.prev_steer_ts_ns + (interval_ns * 2) // 3 + cloudlog.error("PADDLE OFL: Ξ”after=%.1fms Ξ”before=%.1fms credits=%.3f thresh=%.1f timer=%d", + (now_nanos - self.last_steer_ts_ns) * 1e-6, + (now_nanos - self.prev_steer_ts_ns) * 1e-6, + self.spoof_accum, + OVERFLOW_THRESH, + self.regen_paddle_timer) + # Two-sided spacing relative to steer + next_steer_ts_ns = self.last_steer_ts_ns + interval_ns if interval_ns > 0 else 0 + delta_after_ns = now_nanos - self.last_steer_ts_ns + delta_before_ns = (next_steer_ts_ns - now_nanos) if interval_ns > 0 else 1_000_000_000 + if (CS.out.vEgo > 2.68 + and now_nanos >= (slot2_ns - PADDLE_SLOT_EARLY_NS) + and delta_after_ns >= gap_ns + and delta_before_ns >= gap_ns): + # Non-blocking 1 ms spacing for paddle frames + if now_nanos - self.last_paddle_ts_ns >= PADDLE_NONBLOCK_GAP_NS: + paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True)) + paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True)) + self.last_paddle_ts_ns = now_nanos + self.last_spoof_ts_ns = now_nanos + self.spoof_over_sent = True + self.spoof_accum -= OVERFLOW_THRESH + # === End Spoof scheduling === + + # === Off-pulse scheduling on regen release === + if not raw_regen_active and self.last_regen_active: + # schedule two off-slots at 1/3 and 2/3 of the last steer interval + if self.prev_steer_ts_ns and self.last_steer_ts_ns: + intv = self.last_steer_ts_ns - self.prev_steer_ts_ns + self.off_schedule_ns = [ + self.prev_steer_ts_ns + intv // 3, + self.prev_steer_ts_ns + (2 * intv) // 3 + ] + self.off_sent = [False, False] + + if hasattr(self, "off_schedule_ns"): + for i, t_ns in enumerate(self.off_schedule_ns): + if not self.off_sent[i] and now_nanos >= (t_ns - PADDLE_SLOT_EARLY_NS): + cloudlog.error("PADDLE OFF %d: Ξ”after=%.1fms Ξ”to_slot=%.1fms timer=%d", + i, + (now_nanos - self.last_steer_ts_ns) * 1e-6, + (now_nanos - t_ns) * 1e-6, + self.regen_paddle_timer) + # Two-sided spacing to steer before sending + interval_ns = self.last_steer_ts_ns - self.prev_steer_ts_ns + gap_ns = (PADDLE_STEER_GAP_MIN_NS if interval_ns <= 0 else + max(PADDLE_STEER_GAP_MIN_NS, + min(PADDLE_STEER_GAP_MAX_NS, + min((interval_ns // 2) - PADDLE_SLOT_EARLY_NS, PADDLE_GAP_TARGET_NS)))) + next_steer_ts_ns = self.last_steer_ts_ns + interval_ns if interval_ns > 0 else 0 + delta_after_ns = now_nanos - self.last_steer_ts_ns + delta_before_ns = (next_steer_ts_ns - now_nanos) if interval_ns > 0 else 1_000_000_000 + if (delta_after_ns >= gap_ns and delta_before_ns >= gap_ns): + # Non-blocking 1 ms spacing for paddle frames + if now_nanos - self.last_paddle_ts_ns >= PADDLE_NONBLOCK_GAP_NS: + paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, False)) + paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, False)) + self.last_paddle_ts_ns = now_nanos + self.off_sent[i] = True + # clean up once both off pulses are sent + if hasattr(self, "off_sent") and all(self.off_sent): + del self.off_schedule_ns + del self.off_sent + # === End off-pulse scheduling === # Steering (Active: 50Hz, inactive: 10Hz) steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP @@ -112,11 +304,27 @@ class CarController(CarControllerBase): else: apply_steer = 0 + # shift previous steer timestamp + self.prev_steer_ts_ns = self.last_steer_ts_ns + self.last_steer_ts_ns = now_nanos self.last_steer_frame = self.frame self.apply_steer_last = apply_steer idx = self.lka_steering_cmd_counter % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) + # Update regen_active state and last_regen_paddle_pressed for next loop + self.last_regen_active = regen_active + self.last_regen_paddle_pressed = self.regen_paddle_pressed + + if paddle_sends: + interval_ns = self.last_steer_ts_ns - self.prev_steer_ts_ns + flush_gap_ns = (PADDLE_STEER_GAP_MIN_NS if interval_ns <= 0 else + max(PADDLE_STEER_GAP_MIN_NS, + min(PADDLE_STEER_GAP_MAX_NS, + min((interval_ns // 2) - PADDLE_SLOT_EARLY_NS, PADDLE_GAP_TARGET_NS)))) + if now_nanos - self.last_steer_ts_ns >= flush_gap_ns: + can_sends.extend(paddle_sends) + if self.CP.openpilotLongitudinalControl: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: @@ -142,20 +350,15 @@ class CarController(CarControllerBase): self.apply_brake = int(min(-100 * frogpilot_toggles.stopAccel, self.params.MAX_BRAKE)) else: # Normal operation - if self.CP.carFingerprint in EV_CAR: - self.params.update_ev_gas_brake_threshold(CS.out.vEgo) - self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(brake_accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) - else: - self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) # Don't allow any gas above inactive regen while stopping # FIXME: brakes aren't applied immediately when enabling at a stop if stopping: self.apply_gas = self.params.INACTIVE_REGEN if self.CP.carFingerprint in CC_ONLY_CAR: # gas interceptor only used for full long control on cars without ACC - interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive) + interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo) if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill: # "Tap" the accelerator pedal to re-engage ACC @@ -191,7 +394,7 @@ class CarController(CarControllerBase): # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, acc_engaged, at_full_stop)) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, - idx, CC.enabled, near_stop, at_full_stop, self.CP)) + idx, CC.enabled, near_stop, at_full_stop, self.CP)) # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw @@ -202,22 +405,18 @@ class CarController(CarControllerBase): accel += self.accel_g # Radar needs to know current speed and yaw rate (50hz), - # and that ADAS is alive (10hz) + # and that ADAS is alive (5hz, previously 10hz) if not self.CP.radarUnavailable: tt = self.frame * DT_CTRL - time_and_headlights_step = 10 + time_and_headlights_step = 20 if self.frame % time_and_headlights_step == 0: idx = (self.frame // time_and_headlights_step) % 4 can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) - - speed_and_accelerometer_step = 2 - if self.frame % speed_and_accelerometer_step == 0: - idx = (self.frame // speed_and_accelerometer_step) % 4 can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) - if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: + if self.CP.networkLocation == NetworkLocation.gateway and self.frame % (self.params.ADAS_KEEPALIVE_STEP * 2) == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # TODO: integrate this with the code block below? @@ -245,7 +444,7 @@ class CarController(CarControllerBase): if self.CP.networkLocation == NetworkLocation.fwdCamera: # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1 - if self.frame % 10 == 0: + if self.frame % 20 == 0: can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) new_actuators = actuators.as_builder() diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f405d2092..68015ac06 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import mean from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR +from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR, CC_REGEN_PADDLE_CAR TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation @@ -56,6 +56,13 @@ class CarState(CarStateBase): self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 if self.loopback_lka_steering_cmd_updated: self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"] + + # Track timestamps for OEM PRNDL2 and Regen Paddle messages (used to sync spoofing timing) + self.prndl2_ts_nanos = pt_cp.ts_nanos["ECMPRDNL2"]["PRNDL2"] + if self.CP.carFingerprint in CC_REGEN_PADDLE_CAR: + self.regen_paddle_ts_nanos = pt_cp.ts_nanos["EBCMRegenPaddle"]["RegenPaddle"] + else: + self.regen_paddle_ts_nanos = 0 if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value: self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] @@ -71,10 +78,7 @@ class CarState(CarStateBase): # sample rear wheel speeds, standstill=True if ECM allows engagement with brake ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD - if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: - ret.gearShifter = self.parse_gear_shifter("T") - else: - ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) if self.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value: ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 @@ -96,7 +100,7 @@ class CarState(CarStateBase): if self.CP.enableGasInterceptor: ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. - threshold = 10 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting. + threshold = 23 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 595 threshold = 23.65. Set lower to avoid panda blocking messages and GasInterceptor faulting. ret.gasPressed = ret.gas > threshold else: ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. @@ -169,7 +173,7 @@ class CarState(CarStateBase): ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 - # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint in SDGM_CAR: self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] @@ -230,7 +234,7 @@ class CarState(CarStateBase): ] else: messages += [ - ("ECMPRDNL2", 10), + ("ECMPRDNL2", 40), ("AcceleratorPedal2", 33), ("ECMEngineStatus", 100), ("BCMTurnSignals", 1), @@ -252,7 +256,7 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.direct: messages += [ - ("EBCMRegenPaddle", 50), + ("EBCMRegenPaddle", 40), ("EVDriveMode", 0), ] diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 32a0ca761..3bbf2969b 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -177,6 +177,33 @@ def create_lka_icon_command(bus, active, critical, steer): dat = b"\x00\x00\x00" return make_can_msg(0x104c006c, dat, bus) +def create_prndl2_command(packer, bus, press_regen_paddle): + prndl2_value = 7 if press_regen_paddle else 6 + manual_mode = 1 if press_regen_paddle else 0 + values = { + "Byte0": 0x0C, + "Byte1": 0x0C, + "Byte2": 0x00, + "PRNDL2": prndl2_value, + "Byte4": 0x00, + "ManualMode": manual_mode, + "TransmissionState": 1, + "Byte7": 0x00 + } + return packer.make_can_msg("ECMPRDNL2", bus, values) + +def create_regen_paddle_command(packer, bus, press_regen_paddle): + regen_paddle_value = 2 if press_regen_paddle else 0 + values = { + "RegenPaddle": regen_paddle_value, + "Byte1": 0, + "Byte2": 0, + "Byte3": 0, + "Byte4": 0, + "Byte5": 0, + "Byte6": 0 + } + return packer.make_can_msg("EBCMRegenPaddle", bus, values) def create_gm_cc_spam_command(packer, controller, CS, actuators): if controller.params_.get_bool("IsMetric"): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 83c13ced9..632c3dfa7 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -27,8 +27,8 @@ CAM_MSG = 0x320 # AEBCmd ACCELERATOR_POS_MSG = 0xbe NON_LINEAR_TORQUE_PARAMS = { - CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], - CAR.CHEVROLET_BOLT_CC: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], + CAR.CHEVROLET_BOLT_EUV: [1.8, 1.1, 0.280, -0.045], + CAR.CHEVROLET_BOLT_CC: [1.8, 1.1, 0.280, -0.045], CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772], CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122] } @@ -60,10 +60,10 @@ class CarInterface(CarInterfaceBase): # This has big effect on the stability about 0 (noise when going straight) non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint) assert non_linear_torque_params, "The params are not defined" - a, b, c, _ = non_linear_torque_params + a, b, c, d = non_linear_torque_params sig_input = a * lateral_acceleration sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5) - steer_torque = (sig * b) + (lateral_acceleration * c) + steer_torque = (sig * b) + (lateral_acceleration * c) + d return float(steer_torque) lataccel_values = np.arange(-5.0, 5.0, 0.01) @@ -100,13 +100,15 @@ class CarInterface(CarInterfaceBase): if PEDAL_MSG in fingerprint[0]: ret.enableGasInterceptor = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR + # When a pedal interceptor is present, always use normal longitudinal (block stock cruise) + experimental_long = False if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct else: ret.transmissionType = TransmissionType.automatic - ret.longitudinalTuning.kiBP = [5., 35.] + ret.longitudinalTuning.kiBP = [5., 35., 60.] if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR @@ -118,13 +120,14 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 10 * CV.KPH_TO_MS # Tuning for experimental long - ret.longitudinalTuning.kiV = [2.0, 1.5] + ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5] ret.vEgoStopping = 0.1 ret.vEgoStarting = 0.1 - ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling + ret.stoppingDecelRate = 1.0 # reach brake quickly after enabling ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 + ret.stopAccel = -0.25 if ret.experimentalLongitudinalAvailable and experimental_long: ret.pcmCruise = False @@ -132,7 +135,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG elif candidate in SDGM_CAR: - ret.longitudinalTuning.kiV = [0., 0.] # TODO: tuning + ret.longitudinalTuning.kiV = [0., 0., 0.] # TODO: tuning ret.experimentalLongitudinalAvailable = False ret.networkLocation = NetworkLocation.fwdCamera ret.pcmCruise = True @@ -151,7 +154,7 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 7 * CV.MPH_TO_MS # Tuning - ret.longitudinalTuning.kiV = [2.4, 1.5] + ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5] if ret.enableGasInterceptor: # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits @@ -202,6 +205,7 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + ret.lateralTuning.torque.kp = 1.0 if ret.enableGasInterceptor: # ACC Bolts use pedal for full longitudinal control, not just sng @@ -267,13 +271,13 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.autoResumeSng = True - if candidate in CC_ONLY_CAR: + if candidate in CC_ONLY_CAR: #pedal interceptor tuning ret.flags |= GMFlags.PEDAL_LONG.value ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG # Note: Low speed, stop and go not tested. Should be fairly smooth on highway - ret.longitudinalTuning.kiBP = [0.0, 5., 35.] - ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5] - ret.longitudinalTuning.kf = 0.15 + ret.longitudinalTuning.kiBP = [0., 3., 6., 35.] + ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33] + ret.longitudinalTuning.kf = 0.25 ret.stoppingDecelRate = 0.8 else: # Pedal used for SNG, ACC for longitudinal control otherwise ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG @@ -290,16 +294,15 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long ret.pcmCruise = False - ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate) - - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s - ret.longitudinalActuatorDelay = 1. # TODO: measure this - - ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph - ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.1] + if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning + ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph + ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s + ret.longitudinalActuatorDelay = 1. # TODO: measure this + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.1] + ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate) if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index b75e3c24d..52ab348dd 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -33,51 +33,47 @@ class CarControllerParams: # Our controller should still keep the 2 second average above # -3.5 m/s^2 as per planner limits ACCEL_MAX = 2. # m/s^2 + ACCEL_MAX_PLUS = 4. # m/s^2 ACCEL_MIN = -4. # m/s^2 def __init__(self, CP): # Gas/brake lookups - self.ZERO_GAS = 2048 # Coasting + self.ZERO_GAS = 6144 # Coasting self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen - if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR: - self.MAX_GAS = 3400 - self.MAX_ACC_REGEN = 1514 - self.INACTIVE_REGEN = 1554 + if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR and CP.carFingerprint != CAR.CHEVROLET_BOLT_EUV: + self.MAX_GAS = 7496 + self.MAX_GAS_PLUS = 8848 + self.MAX_ACC_REGEN = 5610 + self.INACTIVE_REGEN = 5650 # Camera ACC vehicles have no regen while enabled. # Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly - max_regen_acceleration = 0. + self.max_regen_acceleration = 0. elif CP.carFingerprint in SDGM_CAR: - self.MAX_GAS = 3400 - self.MAX_ACC_REGEN = 1514 - self.INACTIVE_REGEN = 1554 - max_regen_acceleration = 0. + self.MAX_GAS = 7496 + self.MAX_GAS_PLUS = 7496 + self.MAX_ACC_REGEN = 7110 + self.INACTIVE_REGEN = 5650 + self.max_regen_acceleration = 0. else: - self.MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. - self.MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen - self.INACTIVE_REGEN = 1404 + self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill. + self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max + self.MAX_ACC_REGEN = 7110 # Increased for stronger regen braking + self.INACTIVE_REGEN = 5500 # ICE has much less engine braking force compared to regen in EVs, # lower threshold removes some braking deadzone - max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1 + self.max_regen_acceleration = -3. if CP.carFingerprint in EV_CAR else -0.1 # More aggressive regen for EVs - self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX] + self.GAS_LOOKUP_BP = [self.max_regen_acceleration, 0., self.ACCEL_MAX] + self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS] self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS] + self.GAS_LOOKUP_V_PLUS = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS_PLUS] - self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration] + self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration] self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] - # determined by letting Volt regen to a stop in L gear from 89mph, - # and by letting off gas and allowing car to creep, for determining - # the positive threshold values at very low speed - EV_GAS_BRAKE_THRESHOLD_BP = [1.29, 1.52, 1.55, 1.6, 1.7, 1.8, 2.0, 2.2, 2.5, 5.52, 9.6, 20.5, 23.5, 35.0] # [m/s] - EV_GAS_BRAKE_THRESHOLD_V = [0.0, -0.14, -0.16, -0.18, -0.215, -0.255, -0.32, -0.41, -0.5, -0.72, -0.895, -1.125, -1.145, -1.16] # [m/s^s] - - def update_ev_gas_brake_threshold(self, v_ego): - gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V) - self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX] - self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold] @dataclass @@ -198,15 +194,15 @@ class CAR(Platforms): CHEVROLET_SUBURBAN.specs, ) GMC_YUKON_CC = GMPlatformConfig( - [GMCarDocs("GMC Yukon - No-ACC")], + [GMCarDocs("GMC Yukon No ACC")], CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4), ) CADILLAC_CT6_CC = GMPlatformConfig( - [GMCarDocs("Cadillac CT6 - No-ACC")], + [GMCarDocs("Cadillac CT6 No ACC")], CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4), ) CHEVROLET_TRAILBLAZER_CC = GMPlatformConfig( - [GMCarDocs("Chevrolet Trailblazer 2021-22 - No-ACC")], + [GMCarDocs("Chevrolet Trailblazer 2021-22")], CHEVROLET_TRAILBLAZER.specs, ) CADILLAC_XT4 = GMPlatformConfig( @@ -214,7 +210,7 @@ class CAR(Platforms): CarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4), ) CADILLAC_XT5_CC = GMPlatformConfig( - [GMCarDocs("Cadillac XT5 - No-ACC")], + [GMCarDocs("Cadillac XT5 No ACC")], CarSpecs(mass=1810, wheelbase=2.86, steerRatio=16.34, centerToFrontRatio=0.5), ) CHEVROLET_TRAVERSE = GMPlatformConfig( @@ -226,7 +222,7 @@ class CAR(Platforms): CarSpecs(mass=2050, wheelbase=2.86, steerRatio=16.0, centerToFrontRatio=0.5), ) CHEVROLET_MALIBU_CC = GMPlatformConfig( - [GMCarDocs("Chevrolet Malibu 2023 - No-ACC")], + [GMCarDocs("Chevrolet Malibu 2023 No ACC")], CarSpecs(mass=1450, wheelbase=2.8, steerRatio=15.8, centerToFrontRatio=0.4), ) CHEVROLET_TRAX = GMPlatformConfig( @@ -315,6 +311,7 @@ FW_QUERY_CONFIG = FwQueryConfig( EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC} CC_ONLY_CAR = {CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_EQUINOX_CC, CAR.CHEVROLET_SUBURBAN_CC, CAR.GMC_YUKON_CC, CAR.CADILLAC_CT6_CC, CAR.CHEVROLET_TRAILBLAZER_CC, CAR.CADILLAC_XT5_CC, CAR.CHEVROLET_MALIBU_CC} +CC_REGEN_PADDLE_CAR = {CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_BOLT_EUV} # CC_ONLY_CAR = set(c for c in CAR if str(c).endswith('_CC')) # We're integrated at the Safety Data Gateway Module on these cars diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4dc691ead..7f925f8e6 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -52,6 +52,7 @@ GEAR_SHIFTER_MAP: dict[str, car.CarState.GearShifter] = { 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, 'S': GearShifter.sport, 'SPORT': GearShifter.sport, 'L': GearShifter.low, 'LOW': GearShifter.low, + 'L2': GearShifter.low, 'L3': GearShifter.low, 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, } diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 19cc147d7..620abd51e 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -225,7 +225,7 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: return 0.0 -def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): +def get_accel_from_plan_tomb_raider(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): if len(speeds) == len(t_idxs): v_now = speeds[0] a_now = accels[0] diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 63950d4c3..7ee3a4aba 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,6 +4,8 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.selfdrive.car.gm.values import CarControllerParams CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -85,17 +87,48 @@ def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_t return long_control_state - class LongControl: def __init__(self, CP): self.CP = CP self.long_control_state = LongCtrlState.off + self.experimental_mode = False self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL, + pos_p_limit=None) self.v_pid = 0.0 + self._mode_setup() self.last_output_accel = 0.0 + + + def update_mpc_mode(self, experimental_mode): + new_mode = 'blended' if experimental_mode else 'acc' + + if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc': + self.mode_transition_timer = 0.0 + + if new_mode != self.current_mode: + self.prev_mode = self.current_mode + self.transitioning = True + self.mode_transition_timer = 0.0 + self.mode_transition_filter.x = self.last_output_accel + + self.current_mode = new_mode + + if self.transitioning: + self.mode_transition_timer += DT_CTRL + if self.mode_transition_timer >= self.mode_transition_duration: + self.transitioning = False + + def _mode_setup(self): + self.prev_mode = 'acc' + self.current_mode = 'acc' + self.mode_transition_filter = FirstOrderFilter(0.0, 0.5, DT_CTRL) + self.mode_transition_timer = 0.0 + self.mode_transition_duration = 1.0 + self.transitioning = False + def reset(self): self.pid.reset() @@ -124,8 +157,23 @@ class LongControl: else: # LongCtrlState.pid error = a_target - CS.aEgo - output_accel = self.pid.update(error, speed=CS.vEgo, - feedforward=a_target) + self.update_mpc_mode(self.experimental_mode) + raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target) + + + if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': + if raw_output_accel < 0 and raw_output_accel < self.last_output_accel: + progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration) + # Soften transition at low urgency, but keep sharp for high decel + # 20% smoother for chill decel (lower exponent) + urgency = abs(raw_output_accel / CarControllerParams.ACCEL_MIN) + urgency_smooth = min(1.0, urgency ** 0.4) # 20% smoother for chill decel + blend_factor = 1.0 - (1.0 - progress) * (1.0 - urgency_smooth) + output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * blend_factor + else: + output_accel = raw_output_accel + else: + output_accel = raw_output_accel self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index accb441e5..5b08f403a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,11 +3,14 @@ import os import time import numpy as np from cereal import log -from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX +from openpilot.common.numpy_fast import clip, interp from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.conversions import Conversions as CV # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function +from openpilot.selfdrive.car.interfaces import ACCEL_MIN if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -30,12 +33,54 @@ COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 -X_EGO_OBSTACLE_COST = 3. +# ===== VOACC SPEED-BASED TUNING PARAMETERS ===== +# City: Emergency-responsive | Highway: Rubber-banding prevention +# Speed ranges: [0-35, 35-55, 55-70, 70+ mph] + +# SPEED BREAKPOINTS (mph) +SPEED_BREAKPOINTS = [0, 35, 55, 70] # 4 ranges: 0-35, 35-55, 55-70, 70+ + +# ===== CHANGE THESE VALUES FOR DIFFERENT SPEEDS ===== + +# RESPONSIVENESS TO LEAD CARS (Lower = More responsive, Higher = More stable) +# [City Emergency, Urban Hwy, Rural Hwy, High Speed] +X_EGO_OBSTACLE_COSTS = [3.0, 3.0, 2.5, 2.0] # Less aggressive at low speeds, closer to original + +# JERK CONTROL (Lower = More jerky/responsive, Higher = Smoother/conservative) +# [City Emergency, Urban Hwy, Rural Hwy, High Speed] +J_EGO_COSTS = [5.0, 4.75, 4.5, 4.0] # Reverted to original 5.0 at low speeds + +# ACCELERATION CHANGE PENALTIES (Lower = More responsive, Higher = Smoother) +# [City Emergency, Urban Hwy, Rural Hwy, High Speed] +A_CHANGE_COSTS = [200, 195, 180, 170] # Reverted to original 200 at low speeds + +# SMOOTHING FILTERS - Speed-adaptive for optimal responsiveness +# Lower = More responsive, Higher = Smoother +LEAD_FILTER_TIME_LOW = 0.8 # Under 40 mph: Fast response for city emergency braking +LEAD_FILTER_TIME_HIGH = 1.2 # Over 40 mph: Faster response to prevent highway gaps +SPEED_FILTER_THRESHOLD = 40 * CV.MPH_TO_MS # 40 mph threshold + +# DISTANCE ADAPTATION STRENGTH (How much penalties increase when close to lead) +# [City, Urban Hwy, Rural Hwy, High Speed] +DIST_ADAPTS = [0.04, 0.06, 0.06, 0.05] # Balanced across speeds + +# ===== END TUNING PARAMETERS ===== + +# Function to get parameter value based on current speed +def get_speed_based_param(speed_mph, param_array): + """Get parameter value based on current speed using smooth interpolation""" + return np.interp(speed_mph, SPEED_BREAKPOINTS, param_array) + +# Current active values (set based on speed) +X_EGO_OBSTACLE_COST = 2.75 +J_EGO_COST = 5.5 +A_CHANGE_COST = 250.0 +LEAD_FILTER_TIME = 2.0 +DIST_ADAPT = 0.06 + X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. -J_EGO_COST = 5.0 -A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .25 LEAD_DANGER_FACTOR = 0.75 @@ -55,9 +100,6 @@ T_IDXS = np.array(T_IDXS_LST) FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 6.0 -CRUISE_MIN_ACCEL = -1.2 -CRUISE_MAX_ACCEL = 1.6 def get_jerk_factor(aggressive_jerk_acceleration=0.5, aggressive_jerk_danger=0.5, aggressive_jerk_speed=0.5, standard_jerk_acceleration=1.0, standard_jerk_danger=1.0, standard_jerk_speed=1.0, @@ -107,7 +149,11 @@ def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) def get_safe_obstacle_distance(v_ego, t_follow): - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + from openpilot.common.params import Params + params = Params() + stop_str = params.get("StopDistance", encoding="utf8") + stop_distance = float(stop_str) if stop_str else 6.0 + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance def desired_follow_distance(v_ego, v_lead, t_follow=None): if t_follow is None: @@ -188,11 +234,12 @@ def gen_long_ocp(): # from an obstacle at every timestep. This obstacle can be a lead car # or other object. In e2e mode we can use x_position targets as a cost # instead. + accel_change = a_ego - prev_a costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), x_ego, v_ego, a_ego, - a_ego - prev_a, + accel_change, j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) @@ -250,8 +297,20 @@ class LongitudinalMpc: self.mode = mode self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) - self.reset() self.source = SOURCES[2] + # Initialize smoothing filters with default time constants + self.current_filter_time = LEAD_FILTER_TIME_LOW + self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt) + self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt) + # Instance variables to avoid global modifications + self.current_x_ego_cost = X_EGO_OBSTACLE_COSTS[0] + self.current_j_ego_cost = J_EGO_COSTS[0] + self.current_a_change_cost = A_CHANGE_COSTS[0] + self.current_dist_adapt = DIST_ADAPTS[0] + # Initialize acceleration limits to prevent AttributeError + self.cruise_min_a = ACCEL_MIN + self.max_a = 1.2 # Default max acceleration + self.reset() def reset(self): # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -298,10 +357,41 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): + def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0): + # Update parameters based on current speed with interpolation for smooth scaling + speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph + + # Use speed-based parameters for smooth scaling across all breakpoints + self.current_x_ego_cost = get_speed_based_param(speed_mph, X_EGO_OBSTACLE_COSTS) + self.current_j_ego_cost = get_speed_based_param(speed_mph, J_EGO_COSTS) + self.current_a_change_cost = get_speed_based_param(speed_mph, A_CHANGE_COSTS) + + # For dist_adapt, start from 0.0 under low speeds while enabling full smooth transitions + dist_adapt_array = [0.0, DIST_ADAPTS[1], DIST_ADAPTS[2], DIST_ADAPTS[3]] + self.current_dist_adapt = get_speed_based_param(speed_mph, dist_adapt_array) + + # Update filter time constants with interp and recreate filters if needed + if speed_mph < 35: + self.current_filter_time = 0.0 + else: + self.current_filter_time = interp(speed_mph, [35, 45], [0.0, LEAD_FILTER_TIME_HIGH]) + if abs(self.current_filter_time - getattr(self, 'prev_filter_time', 0)) > 0.1: # Only update if significant change + # Recreate filters with new time constant while preserving current values + current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0 + current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0 + self.lead_a_filter = FirstOrderFilter(current_a, self.current_filter_time, self.dt) + self.lead_v_filter = FirstOrderFilter(current_v, self.current_filter_time, self.dt) + self.prev_filter_time = self.current_filter_time + + # Adaptive jerk factors for distance with interp scaling + dist_factor = 1.0 + self.current_dist_adapt * (20.0 / max(lead_dist, 5.0)) + acceleration_jerk *= dist_factor + danger_jerk *= dist_factor + speed_jerk *= dist_factor + if self.mode == 'acc': a_change_cost = acceleration_jerk if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk] + cost_weights = [self.current_x_ego_cost, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk] elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 @@ -320,16 +410,34 @@ class LongitudinalMpc: self.solver.set(i, 'x', self.x0) @staticmethod - def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): - a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) - v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) - x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) + def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego=0.0): + speed_mph = v_ego * CV.MS_TO_MPH + bp = [0, 20, 35] + exp_weight = interp(speed_mph, bp, [1.0, 1.0, 0.0]) # Full exp at <20, blend to constant at 35 + + if exp_weight > 0: + # Exponential decay component + a_lead_traj_exp = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) + v_lead_traj_exp = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj_exp), 0.0, 1e8) + x_lead_traj_exp = x_lead + np.cumsum(T_DIFFS * v_lead_traj_exp) + else: + x_lead_traj_exp = np.zeros_like(T_IDXS) + v_lead_traj_exp = np.zeros_like(T_IDXS) + + # Constant acceleration component + v_lead_traj_const = np.clip(v_lead + a_lead * T_IDXS, 0.0, 1e8) + x_lead_traj_const = x_lead + v_lead * T_IDXS + 0.5 * a_lead * T_IDXS**2 + + # Blend based on weight + v_lead_traj = exp_weight * v_lead_traj_exp + (1 - exp_weight) * v_lead_traj_const + x_lead_traj = exp_weight * x_lead_traj_exp + (1 - exp_weight) * x_lead_traj_const + lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead): + def process_lead(self, lead, tracking_lead=True): v_ego = self.x0[1] - if lead is not None and lead.status: + if lead is not None and lead.status and tracking_lead: x_lead = lead.dRel v_lead = lead.vLead a_lead = lead.aLeadK @@ -344,18 +452,29 @@ class LongitudinalMpc: # MPC will not converge if immediate crash is expected # Clip lead distance to what is still possible to brake for min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) - x_lead = np.clip(x_lead, min_x_lead, 1e8) - v_lead = np.clip(v_lead, 0.0, 1e8) - a_lead = np.clip(a_lead, -10., 5.) - lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) + x_lead = clip(x_lead, min_x_lead, 1e8) + v_lead = clip(v_lead, 0.0, 1e8) + a_lead = clip(a_lead, -10., 5.) + # Apply smoothing filters with interp scaling + self.lead_a_filter.update(a_lead) + self.lead_v_filter.update(v_lead) + a_lead = self.lead_a_filter.x + v_lead = self.lead_v_filter.x + lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego) return lead_xv - def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): - v_ego = self.x0[1] - self.status = radarstate.leadOne.status or radarstate.leadTwo.status + def set_accel_limits(self, min_a, max_a): + # TODO this sets a max accel limit, but the minimum limit is only for cruise decel + # needs refactor + self.cruise_min_a = min_a + self.max_a = max_a - lead_xv_0 = self.process_lead(radarstate.leadOne) - lead_xv_1 = self.process_lead(radarstate.leadTwo) + def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, tracking_lead, personality=log.LongitudinalPersonality.standard): + v_ego = self.x0[1] + self.status = lead_one.status and tracking_lead or lead_two.status + + lead_xv_0 = self.process_lead(lead_one, tracking_lead) + lead_xv_1 = self.process_lead(lead_two, v_ego) # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance @@ -364,7 +483,8 @@ class LongitudinalMpc: lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) self.params[:,0] = ACCEL_MIN - self.params[:,1] = ACCEL_MAX + # negative accel constraint causes problems because negative speed is not allowed + self.params[:,1] = max(0.0, self.max_a) # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': @@ -372,9 +492,9 @@ class LongitudinalMpc: # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. - v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) + v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) + v_upper = v_ego + (T_IDXS * self.max_a * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) @@ -415,8 +535,8 @@ class LongitudinalMpc: self.params[:,4] = t_follow self.run() - if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and - radarstate.leadOne.modelProb > 0.9): + lead_probability = lead_one.modelProb + if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and lead_probability > 0.9): self.crash_cnt += 1 else: self.crash_cnt = 0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ee61161f2..77eca62b2 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import math import numpy as np +from openpilot.common.numpy_fast import clip, interp import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV @@ -11,16 +12,15 @@ from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_accel_from_plan +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET, CONTROL_N, get_speed_error, get_accel_from_plan_tomb_raider from openpilot.common.swaglog import cloudlog -from openpilot.frogpilot.common.frogpilot_variables import MINIMUM_LATERAL_ACCELERATION - LON_MPC_STEP = 0.2 # first step is 0.2s +A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] -ALLOW_THROTTLE_THRESHOLD = 0.4 +ALLOW_THROTTLE_THRESHOLD = 0.5 MIN_ALLOW_THROTTLE_SPEED = 2.5 # Lookup table for turns @@ -29,7 +29,7 @@ _A_TOTAL_MAX_BP = [20., 40.] def get_max_accel(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)) + return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py @@ -42,45 +42,93 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel # The lookup table for turns should also be updated if we do this - a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - - if abs(a_y) > MINIMUM_LATERAL_ACCELERATION: - a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) - else: - a_x_allowed = a_target[1] + a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) return [a_target[0], min(a_target[1], a_x_allowed)] +def get_accel_from_plan_classic(CP, speeds, accels, vEgoStopping): + if len(speeds) == CONTROL_N: + v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds) + a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels) + + v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds) + if v_target != v_target_now: + a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now + else: + a_target = a_target_now + + v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds) + else: + v_target = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + should_stop = (v_target < vEgoStopping and + v_target_1sec < vEgoStopping) + return a_target, should_stop + + +def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): + if len(speeds) == CONTROL_N: + v_now = speeds[0] + a_now = accels[0] + + v_target = interp(action_t, CONTROL_N_T_IDX, speeds) + a_target = 2 * (v_target - v_now) / (action_t) - a_now + v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds) + else: + v_target = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + should_stop = (v_target < vEgoStopping and + v_target_1sec < vEgoStopping) + return a_target, should_stop + + class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) - # TODO remove mpc modes when TR released - self.mpc.mode = 'acc' self.fcw = False self.dt = dt self.allow_throttle = True + self.mode = 'acc' + + self.generation = None self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) - self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] - self.output_a_target = 0.0 - self.output_should_stop = False + self.v_model_error = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 + @property + def mlsim(self): + return self.generation in ("v8", "v10", "v11") + + def get_mpc_mode(self) -> str: + """ + Determine the desired MPC mode: if not ML-SIM, MPC should follow self.mode; + otherwise leave MPC.mode unchanged. + """ + # For non-ML-SIM generations, MPC mode tracks self.mode + if not self.mlsim: + return self.mode + # For ML-SIM (v8), preserve the existing MPC mode + return getattr(self.mpc, 'mode', 'acc') + @staticmethod - def parse_model(model_msg, v_ego, taco_tune): + def parse_model(model_msg, model_error, v_ego, taco_tune): if (len(model_msg.position.x) == ModelConstants.IDX_N and len(model_msg.velocity.x) == ModelConstants.IDX_N and len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) + x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC + v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) j = np.zeros(len(T_IDXS_MPC)) else: @@ -90,7 +138,7 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) if taco_tune: - max_lat_accel = np.interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0]) + max_lat_accel = interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0]) curvatures = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0) max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0 v = np.minimum(max_v, v) @@ -101,17 +149,22 @@ class LongitudinalPlanner: throttle_prob = 1.0 return x, v, a, j, throttle_prob - def update(self, sm, classic_longitudinal, frogpilot_toggles): - mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' - if classic_longitudinal: - self.mpc.mode = mode + def update(self, tinygrad_model, sm, frogpilot_toggles): + self.generation = frogpilot_toggles.model_version + if tinygrad_model: + self.mpc.mode = 'acc' + self.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' + else: + self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' + if not self.mlsim: + self.mpc.mode = self.mode if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) else: accel_coast = ACCEL_MAX - v_ego = sm['carState'].vEgo + v_ego = max(sm['carState'].vEgo, sm['carState'].vEgoCluster) v_cruise = sm['frogpilotPlan'].vCruise v_cruise_initialized = sm['controlsState'].vCruise != V_CRUISE_UNSET @@ -126,36 +179,52 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - if mode == 'acc': - accel_clip = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] + if self.mpc.mode == 'acc': + accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - if not sm['frogpilotPlan'].cscControllingSpeed: - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) + accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: - accel_clip = [ACCEL_MIN, ACCEL_MAX] + accel_limits = [ACCEL_MIN, ACCEL_MAX] + accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] if reset_state: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1]) + self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], v_ego, frogpilot_toggles.taco_tune) + # Compute model v_ego error + self.v_model_error = get_speed_error(sm['modelV2'], v_ego) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_clip[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) - accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) + clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) + clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) if force_slow_decel: v_cruise = 0.0 + # clip limits, cannot init MPC outside of bounds + accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) + accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality) + self.lead_one = sm['radarState'].leadOne + self.lead_two = sm['radarState'].leadTwo + + lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0 + self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, + personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist) + self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['frogpilotPlan'].tFollow, frogpilot_toggles, personality=sm['controlsState'].personality) + # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim + dec_mpc_mode = self.get_mpc_mode() + if not self.mlsim: + self.mpc.mode = dec_mpc_mode + self.mpc.update(self.lead_one, self.lead_two, v_cruise, x, v, a, j, sm['frogpilotPlan'].tFollow, + sm['frogpilotPlan'].trackingLead, personality=sm['controlsState'].personality) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) @@ -167,30 +236,20 @@ class LongitudinalPlanner: if self.fcw: cloudlog.info("FCW triggered") + # Safety checks for rubber-banding mitigation + max_jerk = np.max(np.abs(self.mpc.j_solution)) + max_accel_change = np.max(np.abs(np.diff(self.mpc.a_solution))) + if max_jerk > 5.0: # m/s^3 + cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3") + if max_accel_change > 2.0: # m/s^2 + cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2") + # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) + self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - action_t = frogpilot_toggles.longitudinalActuatorDelay + DT_MDL - output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop - - if mode == 'acc': - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc - else: - output_a_target = min(output_a_target_mpc, output_a_target_e2e) - self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - - for idx in range(2): - accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) - self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) - self.prev_accel_clip = accel_clip - - def publish(self, sm, pm): + def publish(self, classic_model, tinygrad_model, sm, pm, frogpilot_toggles): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) @@ -204,13 +263,34 @@ class LongitudinalPlanner: longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() - longitudinalPlan.hasLead = sm['radarState'].leadOne.status + longitudinalPlan.hasLead = self.lead_one.status longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - longitudinalPlan.aTarget = float(self.output_a_target) - longitudinalPlan.shouldStop = bool(self.output_should_stop) + if classic_model: + a_target, should_stop = get_accel_from_plan_classic(self.CP, longitudinalPlan.speeds, + longitudinalPlan.accels, vEgoStopping=frogpilot_toggles.vEgoStopping) + elif tinygrad_model: + action_t = self.CP.longitudinalActuatorDelay + DT_MDL + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_tomb_raider(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, + action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping) + output_a_target_e2e = sm['modelV2'].action.desiredAcceleration + output_should_stop_e2e = sm['modelV2'].action.shouldStop + + # v9 uses a different longitudinal interface; keep MPC-only behavior even in blended mode + if self.mode == 'acc' or self.generation == 'v9': + a_target = output_a_target_mpc + should_stop = output_should_stop_mpc + else: + a_target = min(output_a_target_mpc, output_a_target_e2e) + should_stop = output_should_stop_e2e or output_should_stop_mpc + else: + action_t = self.CP.longitudinalActuatorDelay + DT_MDL + a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, + action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping) + longitudinalPlan.aTarget = float(a_target) + longitudinalPlan.shouldStop = bool(should_stop) or sm['frogpilotPlan'].forcingStopLength < 1 longitudinalPlan.allowBrake = True - longitudinalPlan.allowThrottle = bool(self.allow_throttle) + longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 99142280c..44accf4e2 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -1,8 +1,13 @@ import numpy as np from numbers import Number +from openpilot.common.numpy_fast import clip, interp + + class PIDController: - def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): + def __init__(self, k_p, k_i, k_f=0., k_d=0., + pos_limit=1e308, neg_limit=-1e308, rate=100, + pos_p_limit=None, neg_p_limit=None): self._k_p = k_p self._k_i = k_i self._k_d = k_d @@ -14,8 +19,13 @@ class PIDController: if isinstance(self._k_d, Number): self._k_d = [[0], [self._k_d]] - self.set_limits(pos_limit, neg_limit) + self.pos_limit = pos_limit + self.neg_limit = neg_limit + self.pos_p_limit = pos_p_limit + self.neg_p_limit = neg_p_limit + + self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate self.speed = 0.0 @@ -23,15 +33,23 @@ class PIDController: @property def k_p(self): - return np.interp(self.speed, self._k_p[0], self._k_p[1]) + return 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]) + return 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]) + return interp(self.speed, self._k_d[0], self._k_d[1]) + + @property + def error_integral(self): + return self.i/self.k_i + + def set_limits(self, pos_limit, neg_limit): + self.pos_limit = pos_limit + self.neg_limit = neg_limit def reset(self): self.p = 0.0 @@ -40,25 +58,29 @@ class PIDController: 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): + def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): self.speed = speed + self.p = float(error) * self.k_p + if self.pos_p_limit is not None and self.p > self.pos_p_limit: + self.p = self.pos_p_limit + elif self.neg_p_limit is not None and self.p < self.neg_p_limit: + self.p = self.neg_p_limit self.f = feedforward * self.k_f self.d = error_rate * self.k_d - if not freeze_integrator: - i = self.i + error * self.k_i * self.i_rate + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + if not freeze_integrator: + self.i = self.i + error * self.k_i * self.i_rate - # 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) + # Clip i to prevent exceeding control limits + control_no_i = self.p + self.d + self.f + control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit) + self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) control = self.p + self.i + self.d + self.f - self.control = np.clip(control, self.neg_limit, self.pos_limit) + + self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 6d526976c..899fa094f 100644 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -37,13 +37,11 @@ def plannerd_thread(): # FrogPilot variables frogpilot_toggles = get_frogpilot_toggles() - classic_longitudinal = frogpilot_toggles.classic_longitudinal - while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm, classic_longitudinal, frogpilot_toggles) - longitudinal_planner.publish(sm, pm) + longitudinal_planner.update(False, sm, frogpilot_toggles) + longitudinal_planner.publish(False, False, sm, pm, frogpilot_toggles) publish_ui_plan(sm, pm, longitudinal_planner) # Update FrogPilot variables diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8d4d6d098..30b398e7a 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -18,7 +18,7 @@ from openpilot.common.simple_kalman import KF1D from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpilot_toggles # Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 1.5 +_LEAD_ACCEL_TAU = 0.6 # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -84,7 +84,7 @@ class Track: # Learn if constant acceleration if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU + self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU) else: self.aLeadTau.update(0.0) @@ -173,14 +173,16 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): - lead_v_rel_pred = lead_msg.v[0] - model_v_ego + prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0) + blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK + get_RadarState_from_vision.prev_aLeadK = blended_aLeadK return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_v_rel_pred), - "vLead": float(v_ego + lead_v_rel_pred), - "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": float(lead_msg.a[0]), + "vRel": float(lead_msg.v[0] - model_v_ego), + "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index c02dcf396..83f77eb7c 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -53,7 +53,8 @@ frogpilot_src = ["../../frogpilot/ui/frogpilot_ui.cc", "../../frogpilot/ui/qt/of "../../frogpilot/ui/qt/offroad/navigation_settings.cc", "../../frogpilot/ui/qt/offroad/sounds_settings.cc", "../../frogpilot/ui/qt/offroad/theme_settings.cc", "../../frogpilot/ui/qt/offroad/utilities.cc", "../../frogpilot/ui/qt/offroad/vehicle_settings.cc", "../../frogpilot/ui/qt/offroad/visual_settings.cc", - "../../frogpilot/ui/qt/offroad/wheel_settings.cc", "../../frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc", + "../../frogpilot/ui/qt/offroad/wheel_settings.cc", "../../frogpilot/ui/qt/offroad/expandable_multi_option_dialog.cc", + "../../frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc", "../../frogpilot/ui/qt/onroad/frogpilot_buttons.cc", "../../frogpilot/ui/qt/onroad/frogpilot_onroad.cc", "../../frogpilot/ui/qt/widgets/developer_sidebar.cc", "../../frogpilot/ui/qt/widgets/drive_stats.cc", "../../frogpilot/ui/qt/widgets/model_reviewer.cc", "../../frogpilot/ui/qt/widgets/navigation_functions.cc", diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 1fd781cc2..81d3c5340 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,13 +1,18 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f.img.xz", - "hash": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f", - "hash_raw": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f", + "url": "https://www.dropbox.com/scl/fi/z8gcamb7n78xqb515kfgq/boot.img.xz?rlkey=r2zxothb3pz0q9rtqysr1zhwv&st=f0acze3w&dl=1", + "hash": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a", + "hash_raw": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a", "size": 16029696, "sparse": false, "full_check": true, - "has_ab": true + "has_ab": true, + "alt": { + "hash": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f", + "url": "https://commadist.azureedge.net/agnosupdate/boot-5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f.img.xz", + "size": 16029696 + } }, { "name": "abl", @@ -61,17 +66,17 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz", - "hash": "328e90c62068222dfd98f71dd3f6251fcb962f082b49c6be66ab2699f5db6f4f", - "hash_raw": "1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a", + "url": "https://www.dropbox.com/scl/fi/n22f3eex1z52dbrhhxqry/system.img.xz?rlkey=yw4ult7s3sdm6b7d31hrm3zx8&st=of6m7zis&dl=1", + "hash": "be1c6bb9ee5e06779087b1b81e09b6df61d942566b0f8d4539c452179c661782", + "hash_raw": "a5f84e68d199466fda5c9aead760b90a4cd2d2ef9a418708b9794d95bb03ec5b", "size": 10737418240, "sparse": true, "full_check": false, "has_ab": true, "alt": { - "hash": "bc11d2148f29862ee1326aca2af1cf6bbf5fed831e3f8f6b8f7a0f110dfe8d26", - "url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz", - "size": 4548070000 + "hash": "328e90c62068222dfd98f71dd3f6251fcb962f082b49c6be66ab2699f5db6f4f", + "url": "https://commadist.azureedge.net/agnosupdate/system-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz", + "size": 10737418240 } } ] diff --git a/system/hardware/tici/agnos.py b/system/hardware/tici/agnos.py index 7e3536f77..45ea20304 100755 --- a/system/hardware/tici/agnos.py +++ b/system/hardware/tici/agnos.py @@ -168,18 +168,24 @@ def extract_compressed_image(target_slot_number: int, partition: dict, cloudlog) last_p = p print(f"Installing {partition['name']}: {p}", flush=True) - if raw_hash.hexdigest().lower() != partition['hash_raw'].lower(): - raise Exception(f"Raw hash mismatch '{raw_hash.hexdigest().lower()}'") + written_size = out.tell() + expected_size = partition['size'] + actual_raw_hash = raw_hash.hexdigest().lower() + expected_raw_hash = partition['hash_raw'].lower() + actual_final_hash = downloader.sha256.hexdigest().lower() + expected_final_hash = partition['hash'].lower() - if downloader.sha256.hexdigest().lower() != partition['hash'].lower(): - raise Exception("Uncompressed hash mismatch") + if actual_raw_hash != expected_raw_hash: + raise Exception(f"Raw hash mismatch: got {actual_raw_hash}, expected {expected_raw_hash}") - if out.tell() != partition['size']: - raise Exception("Uncompressed size mismatch") + if actual_final_hash != expected_final_hash: + raise Exception(f"Uncompressed hash mismatch: got {actual_final_hash}, expected {expected_final_hash}") + + if written_size != expected_size: + raise Exception(f"Uncompressed size mismatch: wrote {written_size} bytes, expected {expected_size} bytes") os.sync() - def extract_casync_image(target_slot_number: int, partition: dict, cloudlog): path = get_partition_path(target_slot_number, partition) seed_path = path[:-1] + ('b' if path[-1] == 'a' else 'a') diff --git a/system/manager/manager.py b/system/manager/manager.py index a2a26b444..30cf1e6c6 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -92,6 +92,16 @@ def manager_init() -> None: params.put_bool("IsTestedBranch", build_metadata.tested_channel) params.put_bool("IsReleaseBranch", build_metadata.release_channel) + # One-time migration for HumanAcceleration and HumanFollowing to off + migration_flag_file = "/data/media/0/frogpilot_human_toggles_migrated.flag" + if not os.path.exists(migration_flag_file): + if params.get_bool("HumanAcceleration"): + params.put_bool("HumanAcceleration", False) + if params.get_bool("HumanFollowing"): + params.put_bool("HumanFollowing", False) + with open(migration_flag_file, "w") as f: + f.write("migrated") + # set dongle id reg_res = register(show_spinner=True) if reg_res: