mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-07 22:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Dom

Browse Source
This commit is contained in:
firestar5683
2025-09-29 11:59:36 -05:00
parent 48d5c4d004
commit 1cdf6ccd04
47 changed files with 2321 additions and 1258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+3
View File
@@ -240,6 +240,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"AutomaticallyDownloadModels", PERSISTENT},
{"AutomaticUpdates", PERSISTENT},
{"AvailableModelNames", PERSISTENT},
{"AvailableModelSeries", PERSISTENT},
{"AvailableModels", PERSISTENT},
{"BigMap", PERSISTENT},
{"BlacklistedModels", PERSISTENT},
@@ -398,6 +399,7 @@ std::unordered_map<std::string, uint32_t> 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<std::string, uint32_t> keys = {
{"WarningSoftVolume", PERSISTENT},
{"WheelIcon", PERSISTENT},
{"WheelSpeed", PERSISTENT},
{"StopDistance", PERSISTENT},
{"WheelToDownload", CLEAR_ON_MANAGER_START},
};
frogpilot/assets/download_functions.py
+32 -37
View File
@@ -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}")
frogpilot/assets/model_manager.py
+339 -462
View File
@@ -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)
frogpilot/assets/other_images/frogpilot_boot_logo.png
BIN
View File
Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 912 KiB

After

Width:  |  Height:  |  Size: 503 KiB

frogpilot/assets/theme_manager.py
+3 -3
View File
@@ -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
frogpilot/common/frogpilot_functions.py
+1 -1
View File
@@ -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():
frogpilot/common/frogpilot_variables.py
+92 -72
View File
@@ -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"))
frogpilot/controls/frogpilot_planner.py
+4 -2
View File
@@ -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
frogpilot/controls/lib/conditional_experimental_mode.py
+102 -11
View File
@@ -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
frogpilot/controls/lib/frogpilot_acceleration.py
+60 -22
View File
@@ -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
frogpilot/controls/lib/frogpilot_following.py
+10 -11
View File
@@ -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
frogpilot/navigation/mapd.py
+1 -1
View File
@@ -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")
frogpilot/tinygrad_modeld/constants.py
+6 -2
View File
@@ -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
frogpilot/tinygrad_modeld/fill_model_msg.py
+38 -6
View File
@@ -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
frogpilot/tinygrad_modeld/parse_model_outputs.py
+38 -30
View File
@@ -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]:
frogpilot/tinygrad_modeld/tinygrad_modeld.py
+174 -145
View File
@@ -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
frogpilot/ui/qt/offroad/expandable_multi_option_dialog.cc
+206
View File
@@ -0,0 +1,206 @@
#include "frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h"
#include <QPushButton>
#include <QButtonGroup>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QScrollBar>
#include <QTimer>
#include "selfdrive/ui/qt/widgets/scrollview.h"
ExpandableMultiOptionDialog::ExpandableMultiOptionDialog(const QString &prompt_text,
const QMap<QString, QStringList> &seriesToModels,
const QString &current, 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<QString, QStringList> &seriesToModels,
const QString &current, QWidget *parent) {
ExpandableMultiOptionDialog d = ExpandableMultiOptionDialog(prompt_text, seriesToModels, current, parent);
if (d.exec()) {
return d.selection;
}
return "";
}
frogpilot/ui/qt/offroad/expandable_multi_option_dialog.h
+28
View File
@@ -0,0 +1,28 @@
#pragma once
#include <QDialog>
#include <QLabel>
#include <QVBoxLayout>
#include <QWidget>
#include <QMap>
#include <QList>
#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<QString, QStringList> &seriesToModels,
const QString &current, QWidget *parent);
static QString getSelection(const QString &prompt_text, const QMap<QString, QStringList> &seriesToModels,
const QString &current, QWidget *parent);
QString selection;
private:
void toggleSeries(const QString &series, QPushButton *headerButton, ScrollView *scrollView);
QMap<QString, QStringList> seriesToModels;
QMap<QString, QWidget*> seriesWidgets;
QMap<QString, bool> seriesExpanded;
};
frogpilot/ui/qt/offroad/model_settings.cc
+286 -181
View File
@@ -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 <QFile>
#include <QJsonDocument>
#include <QJsonObject>
#include <QDoubleSpinBox>
#include <QPushButton>
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<std::tuple<QString, QString, QString, QString>> modelToggles {
{"AutomaticallyDownloadModels", tr("Automatically Download New Models"), tr("<b>Automatically download new driving models</b> as they become available."), ""},
{"DeleteModel", tr("Delete Driving Models"), tr("<b>Delete downloaded driving models</b> to free up storage space."), ""},
{"DownloadModel", tr("Download Driving Models"), tr("<b>Manually download driving models</b> to the device."), ""},
{"ModelRandomizer", tr("Model Randomizer"), tr("<b>Select a random driving model each drive</b> 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("<b>Add or remove driving models from the \"Model Randomizer\" blacklist.</b>"), ""},
{"ManageScores", tr("Manage Model Ratings"), tr("<b>View or reset saved model ratings</b> used by the \"Model Randomizer\"."), ""},
{"SelectModel", tr("Select Driving Model"), tr("<b>Choose which driving model openpilot uses.</b>"), ""},
{"UpdateTinygrad", tr("Update Model Manager"), tr("<b>Update the \"Model Manager\"</b> 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 <b>Model Randomizer</b>'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<QString, QStringList> 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<QString, QStringList> 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<QString, QStringList> 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<QString, QStringList> 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<QString, QStringList> 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<QString> stopDistanceButton{"Reset"};
modelToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, 4.0, 10.0, QString(), std::map<float, QString>(), 0.1, false, {}, stopDistanceButton, false, false);
stopDistanceToggle = static_cast<FrogPilotParamValueButtonControl*>(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<ToggleControl*>(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 <b>Stop Distance</b> 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();
}
frogpilot/ui/qt/offroad/model_settings.h
+2
View File
@@ -55,8 +55,10 @@ private:
QMap<QString, QString> modelFileToNameMap;
QMap<QString, QString> modelFileToNameMapProcessed;
QMap<QString, QString> modelSeriesMap;
QString currentModel;
QStringList availableModelNames;
QStringList availableModelSeries;
};
frogpilot/ui/qt/offroad/visual_settings.cc
+1
View File
@@ -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);
launch_env.sh
+1 -1
View File
@@ -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"
opendbc/gm_global_a_powertrain_generated.dbc
+14 -3
View File
@@ -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"
panda/board/drivers/can_common.h
+3 -3
View File
@@ -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;
}
panda/board/safety.h
+1 -1
View File
@@ -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
panda/board/safety/safety_gm.h
+51 -9
View File
@@ -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);
panda/tests/safety/common.py
+1 -1
View File
@@ -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
panda/tests/safety/test.sh
Regular → Executable
View File
panda/tests/safety/test_gm.py
+63 -16
View File
@@ -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()
selfdrive/assets/img_spinner_comma.png
BIN
View File
Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 390 KiB

After

Width:  |  Height:  |  Size: 418 KiB

selfdrive/car/gm/carcontroller.py
+229 -30
View File
@@ -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()
selfdrive/car/gm/carstate.py
+13 -9
View File
@@ -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),
]
selfdrive/car/gm/gmcan.py
+27
View File
@@ -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"):
selfdrive/car/gm/interface.py
+26 -23
View File
@@ -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
selfdrive/car/gm/values.py
+28 -31
View File
@@ -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
selfdrive/car/interfaces.py
+1
View File
@@ -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,
}
selfdrive/controls/lib/drive_helpers.py
+1 -1
View File
@@ -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]
selfdrive/controls/lib/longcontrol.py
+52 -4
View File
@@ -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
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+152 -32
View File
@@ -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
selfdrive/controls/lib/longitudinal_planner.py
+141 -61
View File
@@ -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)
selfdrive/controls/lib/pid.py
+40 -18
View File
@@ -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
selfdrive/controls/plannerd.py
+2 -4
View File
@@ -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
selfdrive/controls/radard.py
+9 -7
View File
@@ -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),
selfdrive/ui/SConscript
+2 -1
View File
@@ -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",
system/hardware/tici/agnos.json
+15 -10
View File
@@ -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
}
}
]
system/hardware/tici/agnos.py
+13 -7
View File
@@ -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')
system/manager/manager.py
+10
View File
@@ -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: