diff --git a/selfdrive/clearpilot/buttons.py b/scp/buttons.py similarity index 100% rename from selfdrive/clearpilot/buttons.py rename to scp/buttons.py diff --git a/selfdrive/clearpilot/dev/logo.png b/scp/dev/logo.png similarity index 100% rename from selfdrive/clearpilot/dev/logo.png rename to scp/dev/logo.png diff --git a/selfdrive/clearpilot/dev/logo_black.png b/scp/dev/logo_black.png similarity index 100% rename from selfdrive/clearpilot/dev/logo_black.png rename to scp/dev/logo_black.png diff --git a/selfdrive/clearpilot/dev/logo_black.png~ b/scp/dev/logo_black.png~ similarity index 100% rename from selfdrive/clearpilot/dev/logo_black.png~ rename to scp/dev/logo_black.png~ diff --git a/selfdrive/clearpilot/dev/note - resources and notes for dev.txt b/scp/dev/note - resources and notes for dev.txt similarity index 100% rename from selfdrive/clearpilot/dev/note - resources and notes for dev.txt rename to scp/dev/note - resources and notes for dev.txt diff --git a/selfdrive/clearpilot/events.py b/scp/events.py similarity index 100% rename from selfdrive/clearpilot/events.py rename to scp/events.py diff --git a/selfdrive/clearpilot/logo.png b/scp/logo.png similarity index 100% rename from selfdrive/clearpilot/logo.png rename to scp/logo.png diff --git a/selfdrive/clearpilot/logo.png~ b/scp/logo.png~ similarity index 100% rename from selfdrive/clearpilot/logo.png~ rename to scp/logo.png~ diff --git a/selfdrive/clearpilot/settings/advanced.cc b/scp/settings/advanced.cc similarity index 100% rename from selfdrive/clearpilot/settings/advanced.cc rename to scp/settings/advanced.cc diff --git a/selfdrive/clearpilot/settings/advanced.h b/scp/settings/advanced.h similarity index 100% rename from selfdrive/clearpilot/settings/advanced.h rename to scp/settings/advanced.h diff --git a/selfdrive/clearpilot/settings/basic.cc b/scp/settings/basic.cc similarity index 100% rename from selfdrive/clearpilot/settings/basic.cc rename to scp/settings/basic.cc diff --git a/selfdrive/clearpilot/settings/basic.example b/scp/settings/basic.example similarity index 100% rename from selfdrive/clearpilot/settings/basic.example rename to scp/settings/basic.example diff --git a/selfdrive/clearpilot/settings/basic.h b/scp/settings/basic.h similarity index 100% rename from selfdrive/clearpilot/settings/basic.h rename to scp/settings/basic.h diff --git a/selfdrive/clearpilot/settings/basic.h.example b/scp/settings/basic.h.example similarity index 100% rename from selfdrive/clearpilot/settings/basic.h.example rename to scp/settings/basic.h.example diff --git a/selfdrive/clearpilot/settings/defaults.cc b/scp/settings/defaults.cc similarity index 100% rename from selfdrive/clearpilot/settings/defaults.cc rename to scp/settings/defaults.cc diff --git a/selfdrive/clearpilot/settings/defaults.h b/scp/settings/defaults.h similarity index 100% rename from selfdrive/clearpilot/settings/defaults.h rename to scp/settings/defaults.h diff --git a/selfdrive/clearpilot/settings/newmenus b/scp/settings/newmenus similarity index 100% rename from selfdrive/clearpilot/settings/newmenus rename to scp/settings/newmenus diff --git a/selfdrive/clearpilot/settings/notes b/scp/settings/notes similarity index 100% rename from selfdrive/clearpilot/settings/notes rename to scp/settings/notes diff --git a/selfdrive/clearpilot/settings/settings.cc b/scp/settings/settings.cc similarity index 100% rename from selfdrive/clearpilot/settings/settings.cc rename to scp/settings/settings.cc diff --git a/selfdrive/clearpilot/settings/settings.h b/scp/settings/settings.h similarity index 100% rename from selfdrive/clearpilot/settings/settings.h rename to scp/settings/settings.h diff --git a/selfdrive/clearpilot/settings/style.cc b/scp/settings/style.cc similarity index 100% rename from selfdrive/clearpilot/settings/style.cc rename to scp/settings/style.cc diff --git a/selfdrive/clearpilot/settings/style.h b/scp/settings/style.h similarity index 100% rename from selfdrive/clearpilot/settings/style.h rename to scp/settings/style.h diff --git a/selfdrive/clearpilot/settings_sync.py b/scp/settings_sync.py similarity index 100% rename from selfdrive/clearpilot/settings_sync.py rename to scp/settings_sync.py diff --git a/selfdrive/clearpilot/stocklong.py b/scp/stocklong.py similarity index 100% rename from selfdrive/clearpilot/stocklong.py rename to scp/stocklong.py diff --git a/selfdrive/clearpilot/theme/images/button_flag.png b/scp/theme/images/button_flag.png similarity index 100% rename from selfdrive/clearpilot/theme/images/button_flag.png rename to scp/theme/images/button_flag.png diff --git a/selfdrive/clearpilot/theme/images/button_home.png b/scp/theme/images/button_home.png similarity index 100% rename from selfdrive/clearpilot/theme/images/button_home.png rename to scp/theme/images/button_home.png diff --git a/selfdrive/clearpilot/theme/images/button_settings.png b/scp/theme/images/button_settings.png similarity index 100% rename from selfdrive/clearpilot/theme/images/button_settings.png rename to scp/theme/images/button_settings.png diff --git a/selfdrive/clearpilot/theme/images/ready.png b/scp/theme/images/ready.png similarity index 100% rename from selfdrive/clearpilot/theme/images/ready.png rename to scp/theme/images/ready.png diff --git a/selfdrive/clearpilot/theme/images/turn_signal_1.png b/scp/theme/images/turn_signal_1.png similarity index 100% rename from selfdrive/clearpilot/theme/images/turn_signal_1.png rename to scp/theme/images/turn_signal_1.png diff --git a/selfdrive/clearpilot/theme/images/turn_signal_1_red.png b/scp/theme/images/turn_signal_1_red.png similarity index 100% rename from selfdrive/clearpilot/theme/images/turn_signal_1_red.png rename to scp/theme/images/turn_signal_1_red.png diff --git a/selfdrive/clearpilot/theme/images/turn_signal_2.png b/scp/theme/images/turn_signal_2.png similarity index 100% rename from selfdrive/clearpilot/theme/images/turn_signal_2.png rename to scp/theme/images/turn_signal_2.png diff --git a/selfdrive/clearpilot/theme/images/turn_signal_3.png b/scp/theme/images/turn_signal_3.png similarity index 100% rename from selfdrive/clearpilot/theme/images/turn_signal_3.png rename to scp/theme/images/turn_signal_3.png diff --git a/selfdrive/clearpilot/theme/images/turn_signal_4.png b/scp/theme/images/turn_signal_4.png similarity index 100% rename from selfdrive/clearpilot/theme/images/turn_signal_4.png rename to scp/theme/images/turn_signal_4.png diff --git a/selfdrive/clearpilot/theme/sounds/disengage.wav b/scp/theme/sounds/disengage.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/disengage.wav rename to scp/theme/sounds/disengage.wav diff --git a/selfdrive/clearpilot/theme/sounds/engage.wav b/scp/theme/sounds/engage.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/engage.wav rename to scp/theme/sounds/engage.wav diff --git a/selfdrive/clearpilot/theme/sounds/firefox.wav b/scp/theme/sounds/firefox.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/firefox.wav rename to scp/theme/sounds/firefox.wav diff --git a/selfdrive/clearpilot/theme/sounds/prompt.wav b/scp/theme/sounds/prompt.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/prompt.wav rename to scp/theme/sounds/prompt.wav diff --git a/selfdrive/clearpilot/theme/sounds/prompt_distracted.wav b/scp/theme/sounds/prompt_distracted.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/prompt_distracted.wav rename to scp/theme/sounds/prompt_distracted.wav diff --git a/selfdrive/clearpilot/theme/sounds/refuse.wav b/scp/theme/sounds/refuse.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/refuse.wav rename to scp/theme/sounds/refuse.wav diff --git a/selfdrive/clearpilot/theme/sounds/warning_immediate.wav b/scp/theme/sounds/warning_immediate.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/warning_immediate.wav rename to scp/theme/sounds/warning_immediate.wav diff --git a/selfdrive/clearpilot/theme/sounds/warning_soft.wav b/scp/theme/sounds/warning_soft.wav similarity index 100% rename from selfdrive/clearpilot/theme/sounds/warning_soft.wav rename to scp/theme/sounds/warning_soft.wav diff --git a/selfdrive/clearpilot/ui/control_settings.cc b/scp/ui/control_settings.cc similarity index 100% rename from selfdrive/clearpilot/ui/control_settings.cc rename to scp/ui/control_settings.cc diff --git a/selfdrive/clearpilot/ui/control_settings.cc.org b/scp/ui/control_settings.cc.org similarity index 100% rename from selfdrive/clearpilot/ui/control_settings.cc.org rename to scp/ui/control_settings.cc.org diff --git a/selfdrive/clearpilot/ui/control_settings.h b/scp/ui/control_settings.h similarity index 100% rename from selfdrive/clearpilot/ui/control_settings.h rename to scp/ui/control_settings.h diff --git a/selfdrive/clearpilot/ui/control_settings.h.org b/scp/ui/control_settings.h.org similarity index 100% rename from selfdrive/clearpilot/ui/control_settings.h.org rename to scp/ui/control_settings.h.org diff --git a/selfdrive/clearpilot/ui/frogpilot_functions.cc b/scp/ui/frogpilot_functions.cc similarity index 100% rename from selfdrive/clearpilot/ui/frogpilot_functions.cc rename to scp/ui/frogpilot_functions.cc diff --git a/selfdrive/clearpilot/ui/frogpilot_functions.cc.org b/scp/ui/frogpilot_functions.cc.org similarity index 100% rename from selfdrive/clearpilot/ui/frogpilot_functions.cc.org rename to scp/ui/frogpilot_functions.cc.org diff --git a/selfdrive/clearpilot/ui/frogpilot_functions.h b/scp/ui/frogpilot_functions.h similarity index 100% rename from selfdrive/clearpilot/ui/frogpilot_functions.h rename to scp/ui/frogpilot_functions.h diff --git a/selfdrive/clearpilot/ui/frogpilot_functions.h.org b/scp/ui/frogpilot_functions.h.org similarity index 100% rename from selfdrive/clearpilot/ui/frogpilot_functions.h.org rename to scp/ui/frogpilot_functions.h.org diff --git a/selfdrive/clearpilot/ui/vehicle_settings.cc b/scp/ui/vehicle_settings.cc similarity index 100% rename from selfdrive/clearpilot/ui/vehicle_settings.cc rename to scp/ui/vehicle_settings.cc diff --git a/selfdrive/clearpilot/ui/vehicle_settings.cc.org b/scp/ui/vehicle_settings.cc.org similarity index 100% rename from selfdrive/clearpilot/ui/vehicle_settings.cc.org rename to scp/ui/vehicle_settings.cc.org diff --git a/selfdrive/clearpilot/ui/vehicle_settings.h b/scp/ui/vehicle_settings.h similarity index 100% rename from selfdrive/clearpilot/ui/vehicle_settings.h rename to scp/ui/vehicle_settings.h diff --git a/selfdrive/clearpilot/ui/vehicle_settings.h.org b/scp/ui/vehicle_settings.h.org similarity index 100% rename from selfdrive/clearpilot/ui/vehicle_settings.h.org rename to scp/ui/vehicle_settings.h.org diff --git a/selfdrive/clearpilot/ui/visual_settings.cc b/scp/ui/visual_settings.cc similarity index 100% rename from selfdrive/clearpilot/ui/visual_settings.cc rename to scp/ui/visual_settings.cc diff --git a/selfdrive/clearpilot/ui/visual_settings.cc.org b/scp/ui/visual_settings.cc.org similarity index 100% rename from selfdrive/clearpilot/ui/visual_settings.cc.org rename to scp/ui/visual_settings.cc.org diff --git a/selfdrive/clearpilot/ui/visual_settings.h b/scp/ui/visual_settings.h similarity index 100% rename from selfdrive/clearpilot/ui/visual_settings.h rename to scp/ui/visual_settings.h diff --git a/selfdrive/clearpilot/ui/visual_settings.h.org b/scp/ui/visual_settings.h.org similarity index 100% rename from selfdrive/clearpilot/ui/visual_settings.h.org rename to scp/ui/visual_settings.h.org diff --git a/selfdrive/clearpilot/weather.cc b/scp/weather.cc similarity index 100% rename from selfdrive/clearpilot/weather.cc rename to scp/weather.cc diff --git a/selfdrive/clearpilot/weather.h b/scp/weather.h similarity index 100% rename from selfdrive/clearpilot/weather.h rename to scp/weather.h diff --git a/selfdrive/assets/compress-images.sh b/selfdrive/assets/compress-images.sh new file mode 100644 index 0000000..a1a4f8b --- /dev/null +++ b/selfdrive/assets/compress-images.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +echo "compressing training guide images" +optipng -o7 -strip all training/* + +# This can sometimes provide smaller images +# mogrify -quality 100 -format jpg training/* diff --git a/selfdrive/assets/img_spinner_comma.png~ b/selfdrive/assets/img_spinner_comma.png~ deleted file mode 100644 index f942d76..0000000 Binary files a/selfdrive/assets/img_spinner_comma.png~ and /dev/null differ diff --git a/selfdrive/assets/strip-svg-metadata.sh b/selfdrive/assets/strip-svg-metadata.sh new file mode 100644 index 0000000..a8b35ea --- /dev/null +++ b/selfdrive/assets/strip-svg-metadata.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# sudo apt install scour + +for svg in $(find icons/ -type f | grep svg$); do + # scour doesn't support overwriting input file + scour $svg --remove-metadata $svg.tmp + mv $svg.tmp $svg +done diff --git a/selfdrive/assets/translations_assets.qrc b/selfdrive/assets/translations_assets.qrc deleted file mode 100644 index b70d213..0000000 --- a/selfdrive/assets/translations_assets.qrc +++ /dev/null @@ -1,15 +0,0 @@ - - -../ui/translations/main_en.qm -../ui/translations/main_de.qm -../ui/translations/main_fr.qm -../ui/translations/main_pt-BR.qm -../ui/translations/main_tr.qm -../ui/translations/main_ar.qm -../ui/translations/main_th.qm -../ui/translations/main_zh-CHT.qm -../ui/translations/main_zh-CHS.qm -../ui/translations/main_ko.qm -../ui/translations/main_ja.qm - - \ No newline at end of file diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 833bf84..9f90149 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -19,7 +19,8 @@ from dataclasses import asdict, dataclass, replace from datetime import datetime from functools import partial from queue import Queue -from typing import Callable, Dict, List, Optional, Set, Union, cast +from typing import cast +from collections.abc import Callable import requests from jsonrpc import JSONRPCResponseManager, dispatcher @@ -55,17 +56,17 @@ WS_FRAME_SIZE = 4096 NetworkType = log.DeviceState.NetworkType -UploadFileDict = Dict[str, Union[str, int, float, bool]] -UploadItemDict = Dict[str, Union[str, bool, int, float, Dict[str, str]]] +UploadFileDict = dict[str, str | int | float | bool] +UploadItemDict = dict[str, str | bool | int | float | dict[str, str]] -UploadFilesToUrlResponse = Dict[str, Union[int, List[UploadItemDict], List[str]]] +UploadFilesToUrlResponse = dict[str, int | list[UploadItemDict] | list[str]] @dataclass class UploadFile: fn: str url: str - headers: Dict[str, str] + headers: dict[str, str] allow_cellular: bool @classmethod @@ -77,9 +78,9 @@ class UploadFile: class UploadItem: path: str url: str - headers: Dict[str, str] + headers: dict[str, str] created_at: int - id: Optional[str] + id: str | None retry_count: int = 0 current: bool = False progress: float = 0 @@ -97,9 +98,9 @@ send_queue: Queue[str] = queue.Queue() upload_queue: Queue[UploadItem] = queue.Queue() low_priority_send_queue: Queue[str] = queue.Queue() log_recv_queue: Queue[str] = queue.Queue() -cancelled_uploads: Set[str] = set() +cancelled_uploads: set[str] = set() -cur_upload_items: Dict[int, Optional[UploadItem]] = {} +cur_upload_items: dict[int, UploadItem | None] = {} def strip_bz2_extension(fn: str) -> str: @@ -127,14 +128,14 @@ class UploadQueueCache: @staticmethod def cache(upload_queue: Queue[UploadItem]) -> None: try: - queue: List[Optional[UploadItem]] = list(upload_queue.queue) + queue: list[UploadItem | None] = list(upload_queue.queue) items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)] Params().put("AthenadUploadQueue", json.dumps(items)) except Exception: cloudlog.exception("athena.UploadQueueCache.cache.exception") -def handle_long_poll(ws: WebSocket, exit_event: Optional[threading.Event]) -> None: +def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None: end_event = threading.Event() threads = [ @@ -206,13 +207,17 @@ def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = Tr break -def cb(sm, item, tid, sz: int, cur: int) -> None: +def cb(sm, item, tid, end_event: threading.Event, sz: int, cur: int) -> None: # Abort transfer if connection changed to metered after starting upload + # or if athenad is shutting down to re-connect the websocket sm.update(0) metered = sm['deviceState'].networkMetered if metered and (not item.allow_cellular): raise AbortTransferException + if end_event.is_set(): + raise AbortTransferException + cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1) @@ -252,7 +257,7 @@ def upload_handler(end_event: threading.Event) -> None: sz = -1 cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count) - response = _do_upload(item, partial(cb, sm, item, tid)) + response = _do_upload(item, partial(cb, sm, item, tid, end_event)) if response.status_code not in (200, 201, 401, 403, 412): cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered) @@ -274,7 +279,7 @@ def upload_handler(end_event: threading.Event) -> None: cloudlog.exception("athena.upload_handler.exception") -def _do_upload(upload_item: UploadItem, callback: Optional[Callable] = None) -> requests.Response: +def _do_upload(upload_item: UploadItem, callback: Callable = None) -> requests.Response: path = upload_item.path compress = False @@ -313,7 +318,7 @@ def getMessage(service: str, timeout: int = 1000) -> dict: @dispatcher.add_method -def getVersion() -> Dict[str, str]: +def getVersion() -> dict[str, str]: return { "version": get_version(), "remote": get_normalized_origin(), @@ -323,7 +328,7 @@ def getVersion() -> Dict[str, str]: @dispatcher.add_method -def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: Optional[str] = None, place_details: Optional[str] = None) -> Dict[str, int]: +def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: str = None, place_details: str = None) -> dict[str, int]: destination = { "latitude": latitude, "longitude": longitude, @@ -335,7 +340,7 @@ def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: Optiona return {"success": 1} -def scan_dir(path: str, prefix: str) -> List[str]: +def scan_dir(path: str, prefix: str) -> list[str]: files = [] # only walk directories that match the prefix # (glob and friends traverse entire dir tree) @@ -355,12 +360,12 @@ def scan_dir(path: str, prefix: str) -> List[str]: return files @dispatcher.add_method -def listDataDirectory(prefix='') -> List[str]: +def listDataDirectory(prefix='') -> list[str]: return scan_dir(Paths.log_root(), prefix) @dispatcher.add_method -def uploadFileToUrl(fn: str, url: str, headers: Dict[str, str]) -> UploadFilesToUrlResponse: +def uploadFileToUrl(fn: str, url: str, headers: dict[str, str]) -> UploadFilesToUrlResponse: # this is because mypy doesn't understand that the decorator doesn't change the return type response: UploadFilesToUrlResponse = uploadFilesToUrls([{ "fn": fn, @@ -371,11 +376,11 @@ def uploadFileToUrl(fn: str, url: str, headers: Dict[str, str]) -> UploadFilesTo @dispatcher.add_method -def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlResponse: +def uploadFilesToUrls(files_data: list[UploadFileDict]) -> UploadFilesToUrlResponse: files = map(UploadFile.from_dict, files_data) - items: List[UploadItemDict] = [] - failed: List[str] = [] + items: list[UploadItemDict] = [] + failed: list[str] = [] for file in files: if len(file.fn) == 0 or file.fn[0] == '/' or '..' in file.fn or len(file.url) == 0: failed.append(file.fn) @@ -414,13 +419,13 @@ def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlRespo @dispatcher.add_method -def listUploadQueue() -> List[UploadItemDict]: +def listUploadQueue() -> list[UploadItemDict]: items = list(upload_queue.queue) + list(cur_upload_items.values()) return [asdict(i) for i in items if (i is not None) and (i.id not in cancelled_uploads)] @dispatcher.add_method -def cancelUpload(upload_id: Union[str, List[str]]) -> Dict[str, Union[int, str]]: +def cancelUpload(upload_id: str | list[str]) -> dict[str, int | str]: if not isinstance(upload_id, list): upload_id = [upload_id] @@ -433,7 +438,7 @@ def cancelUpload(upload_id: Union[str, List[str]]) -> Dict[str, Union[int, str]] return {"success": 1} @dispatcher.add_method -def setRouteViewed(route: str) -> Dict[str, Union[int, str]]: +def setRouteViewed(route: str) -> dict[str, int | str]: # maintain a list of the last 10 routes viewed in connect params = Params() @@ -448,7 +453,7 @@ def setRouteViewed(route: str) -> Dict[str, Union[int, str]]: return {"success": 1} -def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> Dict[str, int]: +def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> dict[str, int]: try: if local_port not in LOCAL_PORT_WHITELIST: raise Exception("Requested local port not whitelisted") @@ -482,7 +487,7 @@ def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local @dispatcher.add_method -def getPublicKey() -> Optional[str]: +def getPublicKey() -> str | None: if not os.path.isfile(Paths.persist_root() + '/comma/id_rsa.pub'): return None @@ -522,7 +527,7 @@ def getNetworks(): @dispatcher.add_method -def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]: +def takeSnapshot() -> str | dict[str, str] | None: from openpilot.system.camerad.snapshot.snapshot import jpeg_write, snapshot ret = snapshot() if ret is not None: @@ -539,7 +544,7 @@ def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]: raise Exception("not available while camerad is started") -def get_logs_to_send_sorted() -> List[str]: +def get_logs_to_send_sorted() -> list[str]: # TODO: scan once then use inotify to detect file creation/deletion curr_time = int(time.time()) logs = [] @@ -746,6 +751,9 @@ def ws_manage(ws: WebSocket, end_event: threading.Event) -> None: onroad_prev = onroad if sock is not None: + # While not sending data, onroad, we can expect to time out in 7 + (7 * 2) = 21s + # offroad, we can expect to time out in 30 + (10 * 3) = 60s + # FIXME: TCP_USER_TIMEOUT is effectively 2x for some reason (32s), so it's mostly unused sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_USER_TIMEOUT, 16000 if onroad else 0) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 7 if onroad else 30) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 7 if onroad else 10) @@ -759,7 +767,7 @@ def backoff(retries: int) -> int: return random.randrange(0, min(128, int(2 ** retries))) -def main(exit_event: Optional[threading.Event] = None): +def main(exit_event: threading.Event = None): try: set_core_affinity([0, 1, 2, 3]) except Exception: diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 486e426..49a88c8 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -23,8 +23,14 @@ def main(): dirty=is_dirty(), device=HARDWARE.get_device_type()) + frogs_go_moo = Params("/persist/params").get_bool("FrogsGoMoo") + try: while 1: + if frogs_go_moo: + time.sleep(60*60*24*365*100) + continue + cloudlog.info("starting athena daemon") proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad')) proc.start() diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index de74a9d..499e7af 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -4,7 +4,6 @@ import json import jwt import random, string from pathlib import Path -from typing import Optional from datetime import datetime, timedelta from openpilot.common.api import api_get @@ -24,12 +23,12 @@ def is_registered_device() -> bool: return dongle not in (None, UNREGISTERED_DONGLE_ID) -def register(show_spinner=False) -> Optional[str]: +def register(show_spinner=False) -> str | None: params = Params() IMEI = params.get("IMEI", encoding='utf8') HardwareSerial = params.get("HardwareSerial", encoding='utf8') - dongle_id: Optional[str] = params.get("DongleId", encoding='utf8') + dongle_id: str | None = params.get("DongleId", encoding='utf8') needs_registration = None in (IMEI, HardwareSerial, dongle_id) pubkey = Path(Paths.persist_root()+"/comma/id_rsa.pub") @@ -49,8 +48,8 @@ def register(show_spinner=False) -> Optional[str]: # Block until we get the imei serial = HARDWARE.get_serial() start_time = time.monotonic() - imei1: Optional[str] = None - imei2: Optional[str] = None + imei1: str | None = None + imei2: str | None = None while imei1 is None and imei2 is None: try: imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) @@ -76,8 +75,8 @@ def register(show_spinner=False) -> Optional[str]: if resp.status_code in (402, 403): cloudlog.info(f"Unable to register device, got {resp.status_code}") dongle_id = ''.join(random.choices(string.ascii_lowercase + string.digits, k=16)) - params.put_bool("FireTheBabysitter", True) - params.put_bool("NoLogging", True) + elif Params("/persist/params").get_bool("FrogsGoMoo"): + dongle_id = "FrogsGoMooDongle" else: dongleauth = json.loads(resp.text) dongle_id = dongleauth["dongle_id"] diff --git a/selfdrive/athena/tests/__init__.py b/selfdrive/athena/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/athena/tests/helpers.py b/selfdrive/athena/tests/helpers.py new file mode 100644 index 0000000..3dd98f0 --- /dev/null +++ b/selfdrive/athena/tests/helpers.py @@ -0,0 +1,65 @@ +import http.server +import socket + + +class MockResponse: + def __init__(self, json, status_code): + self.json = json + self.text = json + self.status_code = status_code + + +class EchoSocket(): + def __init__(self, port): + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.bind(('127.0.0.1', port)) + self.socket.listen(1) + + def run(self): + conn, _ = self.socket.accept() + conn.settimeout(5.0) + + try: + while True: + data = conn.recv(4096) + if data: + print(f'EchoSocket got {data}') + conn.sendall(data) + else: + break + finally: + conn.shutdown(0) + conn.close() + self.socket.shutdown(0) + self.socket.close() + + +class MockApi(): + def __init__(self, dongle_id): + pass + + def get_token(self): + return "fake-token" + + +class MockWebsocket(): + def __init__(self, recv_queue, send_queue): + self.recv_queue = recv_queue + self.send_queue = send_queue + + def recv(self): + data = self.recv_queue.get() + if isinstance(data, Exception): + raise data + return data + + def send(self, data, opcode): + self.send_queue.put_nowait((data, opcode)) + + +class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler): + def do_PUT(self): + length = int(self.headers['Content-Length']) + self.rfile.read(length) + self.send_response(201, "Created") + self.end_headers() diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py new file mode 100644 index 0000000..4850ab9 --- /dev/null +++ b/selfdrive/athena/tests/test_athenad.py @@ -0,0 +1,434 @@ +#!/usr/bin/env python3 +from functools import partial, wraps +import json +import multiprocessing +import os +import requests +import shutil +import time +import threading +import queue +import unittest +from dataclasses import asdict, replace +from datetime import datetime, timedelta +from parameterized import parameterized + +from unittest import mock +from websocket import ABNF +from websocket._exceptions import WebSocketConnectionClosedException + +from cereal import messaging + +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.selfdrive.athena import athenad +from openpilot.selfdrive.athena.athenad import MAX_RETRY_COUNT, dispatcher +from openpilot.selfdrive.athena.tests.helpers import HTTPRequestHandler, MockWebsocket, MockApi, EchoSocket +from openpilot.selfdrive.test.helpers import with_http_server +from openpilot.system.hardware.hw import Paths + + +def seed_athena_server(host, port): + with Timeout(2, 'HTTP Server seeding failed'): + while True: + try: + requests.put(f'http://{host}:{port}/qlog.bz2', data='', timeout=10) + break + except requests.exceptions.ConnectionError: + time.sleep(0.1) + + +with_mock_athena = partial(with_http_server, handler=HTTPRequestHandler, setup=seed_athena_server) + + +def with_upload_handler(func): + @wraps(func) + def wrapper(*args, **kwargs): + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + try: + return func(*args, **kwargs) + finally: + end_event.set() + thread.join() + return wrapper + + +class TestAthenadMethods(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.SOCKET_PORT = 45454 + athenad.Api = MockApi + athenad.LOCAL_PORT_WHITELIST = {cls.SOCKET_PORT} + + def setUp(self): + self.default_params = { + "DongleId": "0000000000000000", + "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private", # noqa: E501 + "GithubUsername": b"commaci", + "AthenadUploadQueue": '[]', + } + + self.params = Params() + for k, v in self.default_params.items(): + self.params.put(k, v) + self.params.put_bool("GsmMetered", True) + + athenad.upload_queue = queue.Queue() + athenad.cur_upload_items.clear() + athenad.cancelled_uploads.clear() + + for i in os.listdir(Paths.log_root()): + p = os.path.join(Paths.log_root(), i) + if os.path.isdir(p): + shutil.rmtree(p) + else: + os.unlink(p) + + # *** test helpers *** + + @staticmethod + def _wait_for_upload(): + now = time.time() + while time.time() - now < 5: + if athenad.upload_queue.qsize() == 0: + break + + @staticmethod + def _create_file(file: str, parent: str = None, data: bytes = b'') -> str: + fn = os.path.join(Paths.log_root() if parent is None else parent, file) + os.makedirs(os.path.dirname(fn), exist_ok=True) + with open(fn, 'wb') as f: + f.write(data) + return fn + + + # *** test cases *** + + def test_echo(self): + assert dispatcher["echo"]("bob") == "bob" + + def test_getMessage(self): + with self.assertRaises(TimeoutError) as _: + dispatcher["getMessage"]("controlsState") + + end_event = multiprocessing.Event() + + pub_sock = messaging.pub_sock("deviceState") + + def send_deviceState(): + while not end_event.is_set(): + msg = messaging.new_message('deviceState') + pub_sock.send(msg.to_bytes()) + time.sleep(0.01) + + p = multiprocessing.Process(target=send_deviceState) + p.start() + time.sleep(0.1) + try: + deviceState = dispatcher["getMessage"]("deviceState") + assert deviceState['deviceState'] + finally: + end_event.set() + p.join() + + def test_listDataDirectory(self): + route = '2021-03-29--13-32-47' + segments = [0, 1, 2, 3, 11] + + filenames = ['qlog', 'qcamera.ts', 'rlog', 'fcamera.hevc', 'ecamera.hevc', 'dcamera.hevc'] + files = [f'{route}--{s}/{f}' for s in segments for f in filenames] + for file in files: + self._create_file(file) + + resp = dispatcher["listDataDirectory"]() + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, files) + + resp = dispatcher["listDataDirectory"](f'{route}--123') + self.assertCountEqual(resp, []) + + prefix = f'{route}' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + prefix = f'{route}--1' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + prefix = f'{route}--1/' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + prefix = f'{route}--1/q' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + def test_strip_bz2_extension(self): + fn = self._create_file('qlog.bz2') + if fn.endswith('.bz2'): + self.assertEqual(athenad.strip_bz2_extension(fn), fn[:-4]) + + @parameterized.expand([(True,), (False,)]) + @with_mock_athena + def test_do_upload(self, compress, host): + # random bytes to ensure rather large object post-compression + fn = self._create_file('qlog', data=os.urandom(10000 * 1024)) + + upload_fn = fn + ('.bz2' if compress else '') + item = athenad.UploadItem(path=upload_fn, url="http://localhost:1238", headers={}, created_at=int(time.time()*1000), id='') + with self.assertRaises(requests.exceptions.ConnectionError): + athenad._do_upload(item) + + item = athenad.UploadItem(path=upload_fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + resp = athenad._do_upload(item) + self.assertEqual(resp.status_code, 201) + + @with_mock_athena + def test_uploadFileToUrl(self, host): + fn = self._create_file('qlog.bz2') + + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", f"{host}/qlog.bz2", {}) + self.assertEqual(resp['enqueued'], 1) + self.assertNotIn('failed', resp) + self.assertLessEqual({"path": fn, "url": f"{host}/qlog.bz2", "headers": {}}.items(), resp['items'][0].items()) + self.assertIsNotNone(resp['items'][0].get('id')) + self.assertEqual(athenad.upload_queue.qsize(), 1) + + @with_mock_athena + def test_uploadFileToUrl_duplicate(self, host): + self._create_file('qlog.bz2') + + url1 = f"{host}/qlog.bz2?sig=sig1" + dispatcher["uploadFileToUrl"]("qlog.bz2", url1, {}) + + # Upload same file again, but with different signature + url2 = f"{host}/qlog.bz2?sig=sig2" + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", url2, {}) + self.assertEqual(resp, {'enqueued': 0, 'items': []}) + + @with_mock_athena + def test_uploadFileToUrl_does_not_exist(self, host): + not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {}) + self.assertEqual(not_exists_resp, {'enqueued': 0, 'items': [], 'failed': ['does_not_exist.bz2']}) + + @with_mock_athena + @with_upload_handler + def test_upload_handler(self, host): + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + # TODO: verify that upload actually succeeded + # TODO: also check that end_event and metered network raises AbortTransferException + self.assertEqual(athenad.upload_queue.qsize(), 0) + + @parameterized.expand([(500, True), (412, False)]) + @with_mock_athena + @mock.patch('requests.put') + @with_upload_handler + def test_upload_handler_retry(self, status, retry, mock_put, host): + mock_put.return_value.status_code = status + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 1 if retry else 0) + + if retry: + self.assertEqual(athenad.upload_queue.get().retry_count, 1) + + @with_upload_handler + def test_upload_handler_timeout(self): + """When an upload times out or fails to connect it should be placed back in the queue""" + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + item_no_retry = replace(item, retry_count=MAX_RETRY_COUNT) + + athenad.upload_queue.put_nowait(item_no_retry) + self._wait_for_upload() + time.sleep(0.1) + + # Check that upload with retry count exceeded is not put back + self.assertEqual(athenad.upload_queue.qsize(), 0) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + # Check that upload item was put back in the queue with incremented retry count + self.assertEqual(athenad.upload_queue.qsize(), 1) + self.assertEqual(athenad.upload_queue.get().retry_count, 1) + + @with_upload_handler + def test_cancelUpload(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, + created_at=int(time.time()*1000), id='id', allow_cellular=True) + athenad.upload_queue.put_nowait(item) + dispatcher["cancelUpload"](item.id) + + self.assertIn(item.id, athenad.cancelled_uploads) + + self._wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 0) + self.assertEqual(len(athenad.cancelled_uploads), 0) + + @with_upload_handler + def test_cancelExpiry(self): + t_future = datetime.now() - timedelta(days=40) + ts = int(t_future.strftime("%s")) * 1000 + + # Item that would time out if actually uploaded + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=ts, id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 0) + + def test_listUploadQueueEmpty(self): + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 0) + + @with_http_server + @with_upload_handler + def test_listUploadQueueCurrent(self, host: str): + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 1) + self.assertTrue(items[0]['current']) + + def test_listUploadQueue(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, + created_at=int(time.time()*1000), id='id', allow_cellular=True) + athenad.upload_queue.put_nowait(item) + + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 1) + self.assertDictEqual(items[0], asdict(item)) + self.assertFalse(items[0]['current']) + + athenad.cancelled_uploads.add(item.id) + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 0) + + def test_upload_queue_persistence(self): + item1 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id1') + item2 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id2') + + athenad.upload_queue.put_nowait(item1) + athenad.upload_queue.put_nowait(item2) + + # Ensure cancelled items are not persisted + athenad.cancelled_uploads.add(item2.id) + + # serialize item + athenad.UploadQueueCache.cache(athenad.upload_queue) + + # deserialize item + athenad.upload_queue.queue.clear() + athenad.UploadQueueCache.initialize(athenad.upload_queue) + + self.assertEqual(athenad.upload_queue.qsize(), 1) + self.assertDictEqual(asdict(athenad.upload_queue.queue[-1]), asdict(item1)) + + @mock.patch('openpilot.selfdrive.athena.athenad.create_connection') + def test_startLocalProxy(self, mock_create_connection): + end_event = threading.Event() + + ws_recv = queue.Queue() + ws_send = queue.Queue() + mock_ws = MockWebsocket(ws_recv, ws_send) + mock_create_connection.return_value = mock_ws + + echo_socket = EchoSocket(self.SOCKET_PORT) + socket_thread = threading.Thread(target=echo_socket.run) + socket_thread.start() + + athenad.startLocalProxy(end_event, 'ws://localhost:1234', self.SOCKET_PORT) + + ws_recv.put_nowait(b'ping') + try: + recv = ws_send.get(timeout=5) + assert recv == (b'ping', ABNF.OPCODE_BINARY), recv + finally: + # signal websocket close to athenad.ws_proxy_recv + ws_recv.put_nowait(WebSocketConnectionClosedException()) + socket_thread.join() + + def test_getSshAuthorizedKeys(self): + keys = dispatcher["getSshAuthorizedKeys"]() + self.assertEqual(keys, self.default_params["GithubSshKeys"].decode('utf-8')) + + def test_getGithubUsername(self): + keys = dispatcher["getGithubUsername"]() + self.assertEqual(keys, self.default_params["GithubUsername"].decode('utf-8')) + + def test_getVersion(self): + resp = dispatcher["getVersion"]() + keys = ["version", "remote", "branch", "commit"] + self.assertEqual(list(resp.keys()), keys) + for k in keys: + self.assertIsInstance(resp[k], str, f"{k} is not a string") + self.assertTrue(len(resp[k]) > 0, f"{k} has no value") + + def test_jsonrpc_handler(self): + end_event = threading.Event() + thread = threading.Thread(target=athenad.jsonrpc_handler, args=(end_event,)) + thread.daemon = True + thread.start() + try: + # with params + athenad.recv_queue.put_nowait(json.dumps({"method": "echo", "params": ["hello"], "jsonrpc": "2.0", "id": 0})) + resp = athenad.send_queue.get(timeout=3) + self.assertDictEqual(json.loads(resp), {'result': 'hello', 'id': 0, 'jsonrpc': '2.0'}) + # without params + athenad.recv_queue.put_nowait(json.dumps({"method": "getNetworkType", "jsonrpc": "2.0", "id": 0})) + resp = athenad.send_queue.get(timeout=3) + self.assertDictEqual(json.loads(resp), {'result': 1, 'id': 0, 'jsonrpc': '2.0'}) + # log forwarding + athenad.recv_queue.put_nowait(json.dumps({'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'})) + resp = athenad.log_recv_queue.get(timeout=3) + self.assertDictEqual(json.loads(resp), {'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'}) + finally: + end_event.set() + thread.join() + + def test_get_logs_to_send_sorted(self): + fl = list() + for i in range(10): + file = f'swaglog.{i:010}' + self._create_file(file, Paths.swaglog_root()) + fl.append(file) + + # ensure the list is all logs except most recent + sl = athenad.get_logs_to_send_sorted() + self.assertListEqual(sl, fl[:-1]) + + +if __name__ == '__main__': + unittest.main() diff --git a/selfdrive/athena/tests/test_athenad_ping.py b/selfdrive/athena/tests/test_athenad_ping.py new file mode 100644 index 0000000..f56fcac --- /dev/null +++ b/selfdrive/athena/tests/test_athenad_ping.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +import subprocess +import threading +import time +import unittest +from typing import cast +from unittest import mock + +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.selfdrive.athena import athenad +from openpilot.selfdrive.manager.helpers import write_onroad_params +from openpilot.system.hardware import TICI + +TIMEOUT_TOLERANCE = 20 # seconds + + +def wifi_radio(on: bool) -> None: + if not TICI: + return + print(f"wifi {'on' if on else 'off'}") + subprocess.run(["nmcli", "radio", "wifi", "on" if on else "off"], check=True) + + +class TestAthenadPing(unittest.TestCase): + params: Params + dongle_id: str + + athenad: threading.Thread + exit_event: threading.Event + + def _get_ping_time(self) -> str | None: + return cast(str | None, self.params.get("LastAthenaPingTime", encoding="utf-8")) + + def _clear_ping_time(self) -> None: + self.params.remove("LastAthenaPingTime") + + def _received_ping(self) -> bool: + return self._get_ping_time() is not None + + @classmethod + def tearDownClass(cls) -> None: + wifi_radio(True) + + def setUp(self) -> None: + self.params = Params() + self.dongle_id = self.params.get("DongleId", encoding="utf-8") + + wifi_radio(True) + self._clear_ping_time() + + self.exit_event = threading.Event() + self.athenad = threading.Thread(target=athenad.main, args=(self.exit_event,)) + + def tearDown(self) -> None: + if self.athenad.is_alive(): + self.exit_event.set() + self.athenad.join() + + @mock.patch('openpilot.selfdrive.athena.athenad.create_connection', new_callable=lambda: mock.MagicMock(wraps=athenad.create_connection)) + def assertTimeout(self, reconnect_time: float, mock_create_connection: mock.MagicMock) -> None: + self.athenad.start() + + time.sleep(1) + mock_create_connection.assert_called_once() + mock_create_connection.reset_mock() + + # check normal behaviour, server pings on connection + with self.subTest("Wi-Fi: receives ping"), Timeout(70, "no ping received"): + while not self._received_ping(): + time.sleep(0.1) + print("ping received") + + mock_create_connection.assert_not_called() + + # websocket should attempt reconnect after short time + with self.subTest("LTE: attempt reconnect"): + wifi_radio(False) + print("waiting for reconnect attempt") + start_time = time.monotonic() + with Timeout(reconnect_time, "no reconnect attempt"): + while not mock_create_connection.called: + time.sleep(0.1) + print(f"reconnect attempt after {time.monotonic() - start_time:.2f}s") + + self._clear_ping_time() + + # check ping received after reconnect + with self.subTest("LTE: receives ping"), Timeout(70, "no ping received"): + while not self._received_ping(): + time.sleep(0.1) + print("ping received") + + @unittest.skipIf(not TICI, "only run on desk") + def test_offroad(self) -> None: + write_onroad_params(False, self.params) + self.assertTimeout(60 + TIMEOUT_TOLERANCE) # based using TCP keepalive settings + + @unittest.skipIf(not TICI, "only run on desk") + def test_onroad(self) -> None: + write_onroad_params(True, self.params) + self.assertTimeout(21 + TIMEOUT_TOLERANCE) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/athena/tests/test_registration.py b/selfdrive/athena/tests/test_registration.py new file mode 100644 index 0000000..e7ad63a --- /dev/null +++ b/selfdrive/athena/tests/test_registration.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +import json +import unittest +from Crypto.PublicKey import RSA +from pathlib import Path +from unittest import mock + +from openpilot.common.params import Params +from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID +from openpilot.selfdrive.athena.tests.helpers import MockResponse +from openpilot.system.hardware.hw import Paths + + +class TestRegistration(unittest.TestCase): + + def setUp(self): + # clear params and setup key paths + self.params = Params() + self.params.clear_all() + + persist_dir = Path(Paths.persist_root()) / "comma" + persist_dir.mkdir(parents=True, exist_ok=True) + + self.priv_key = persist_dir / "id_rsa" + self.pub_key = persist_dir / "id_rsa.pub" + + def _generate_keys(self): + self.pub_key.touch() + k = RSA.generate(2048) + with open(self.priv_key, "wb") as f: + f.write(k.export_key()) + with open(self.pub_key, "wb") as f: + f.write(k.publickey().export_key()) + + def test_valid_cache(self): + # if all params are written, return the cached dongle id + self.params.put("IMEI", "imei") + self.params.put("HardwareSerial", "serial") + self._generate_keys() + + with mock.patch("openpilot.selfdrive.athena.registration.api_get", autospec=True) as m: + dongle = "DONGLE_ID_123" + self.params.put("DongleId", dongle) + self.assertEqual(register(), dongle) + self.assertFalse(m.called) + + def test_no_keys(self): + # missing pubkey + with mock.patch("openpilot.selfdrive.athena.registration.api_get", autospec=True) as m: + dongle = register() + self.assertEqual(m.call_count, 0) + self.assertEqual(dongle, UNREGISTERED_DONGLE_ID) + self.assertEqual(self.params.get("DongleId", encoding='utf-8'), dongle) + + def test_missing_cache(self): + # keys exist but no dongle id + self._generate_keys() + with mock.patch("openpilot.selfdrive.athena.registration.api_get", autospec=True) as m: + dongle = "DONGLE_ID_123" + m.return_value = MockResponse(json.dumps({'dongle_id': dongle}), 200) + self.assertEqual(register(), dongle) + self.assertEqual(m.call_count, 1) + + # call again, shouldn't hit the API this time + self.assertEqual(register(), dongle) + self.assertEqual(m.call_count, 1) + self.assertEqual(self.params.get("DongleId", encoding='utf-8'), dongle) + + def test_unregistered(self): + # keys exist, but unregistered + self._generate_keys() + with mock.patch("openpilot.selfdrive.athena.registration.api_get", autospec=True) as m: + m.return_value = MockResponse(None, 402) + dongle = register() + self.assertEqual(m.call_count, 1) + self.assertEqual(dongle, UNREGISTERED_DONGLE_ID) + self.assertEqual(self.params.get("DongleId", encoding='utf-8'), dongle) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md new file mode 100644 index 0000000..9f9f6c2 --- /dev/null +++ b/selfdrive/car/CARS_template.md @@ -0,0 +1,73 @@ +{% set footnote_tag = '[{}](#footnotes)' %} +{% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' %} +{% set video_icon = '' %} +{# Force hardware column wider by using a blank image with max width. #} +{% set width_tag = '%s
 ' %} +{% set hardware_col_name = 'Hardware Needed' %} +{% set wide_hardware_col_name = width_tag|format(hardware_col_name) -%} + + + +# Supported Cars + +A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. + +# {{all_car_docs | length}} Supported Cars + +|{{Column | map(attribute='value') | join('|') | replace(hardware_col_name, wide_hardware_col_name)}}| +|---|---|---|{% for _ in range((Column | length) - 3) %}{{':---:|'}}{% endfor +%} +{% for car_docs in all_car_docs %} +|{% for column in Column %}{{car_docs.get_column(column, star_icon, video_icon, footnote_tag)}}|{% endfor %} + +{% endfor %} + +### Footnotes +{% for footnote in footnotes %} +{{loop.index}}{{footnote}}
+{% endfor %} + +## Community Maintained Cars +Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). + +# Don't see your car here? + +**openpilot can support many more cars than it currently does.** There are a few reasons your car may not be supported. +If your car doesn't fit into any of the incompatibility criteria here, then there's a good chance it can be supported! We're adding support for new cars all the time. **We don't have a roadmap for car support**, and in fact, most car support comes from users like you! + +### Which cars are able to be supported? + +openpilot uses the existing steering, gas, and brake interfaces in your car. If your car lacks any one of these interfaces, openpilot will not be able to control the car. If your car has [ACC](https://en.wikipedia.org/wiki/Adaptive_cruise_control) and any form of [LKAS](https://en.wikipedia.org/wiki/Automated_Lane_Keeping_Systems)/[LCA](https://en.wikipedia.org/wiki/Lane_centering), then it almost certainly has these interfaces. These features generally started shipping on cars around 2016. Note that manufacturers will often make their own [marketing terms](https://en.wikipedia.org/wiki/Adaptive_cruise_control#Vehicle_models_supporting_adaptive_cruise_control) for these features, such as Hyundai's "Smart Cruise Control" branding of Adaptive Cruise Control. + +If your car has the following packages or features, then it's a good candidate for support. + +| Make | Required Package/Features | +| ---- | ------------------------- | +| Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | +| Ford | Any car with Lane Centering will likely work. | +| Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | +| Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | +| Nissan | Any car with ProPILOT will likely work. | +| Toyota & Lexus | Any car that has Toyota/Lexus Safety Sense with "Lane Departure Alert with Steering Assist (LDA w/SA)" and/or "Lane Tracing Assist (LTA)" will work. Note that LDA without Steering Assist will not work. These features come standard on most newer models. | +| Hyundai, Kia, & Genesis | Any car with Smart Cruise Control (SCC) and Lane Following Assist (LFA) or Lane Keeping Assist (LKAS) will work. LKAS/LFA comes standard on most newer models. Any form of SCC will work, such as NSCC. | +| Chrysler, Jeep, & Ram | Any car with LaneSense and Adaptive Cruise Control will likely work. These come standard on many newer models. | + +### FlexRay + +All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay. + +### Toyota Security + +openpilot does not yet support these Toyota models due to a new message authentication method. +[Vote](https://comma.ai/shop#toyota-security) if you'd like to see openpilot support on these models. + +* Toyota RAV4 Prime 2021+ +* Toyota Sienna 2021+ +* Toyota Venza 2021+ +* Toyota Sequoia 2023+ +* Toyota Tundra 2022+ +* Toyota Highlander 2024+ +* Toyota Corolla Cross 2022+ (only US model) +* Lexus NX 2022+ +* Toyota bZ4x 2023+ +* Subaru Solterra 2023+ + diff --git a/selfdrive/car/README.md b/selfdrive/car/README.md new file mode 100644 index 0000000..2c49cf2 --- /dev/null +++ b/selfdrive/car/README.md @@ -0,0 +1,19 @@ +## Car port structure + +### interface.py +Generic interface to send and receive messages from CAN (controlsd uses this to communicate with car) + +### fingerprints.py +Fingerprints for matching to a specific car + +### carcontroller.py +Builds CAN messages to send to car + +##### carstate.py +Reads CAN from car and builds openpilot CarState message + +##### values.py +Limits for actuation, general constants for cars, and supported car documentation + +##### radar_interface.py +Interface for parsing radar points from the car diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index c90ae50..106aedd 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,11 +1,15 @@ # functions common among cars -from collections import namedtuple -from typing import Dict, List, Optional +from collections import defaultdict, namedtuple +from dataclasses import dataclass +from enum import IntFlag, ReprEnum +from dataclasses import replace import capnp from cereal import car from openpilot.common.numpy_fast import clip, interp +from openpilot.common.utils import Freezable +from openpilot.selfdrive.car.docs_definitions import CarDocs # kg of standard extra cargo to count for drive, gas, etc... @@ -24,9 +28,9 @@ def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: return val_steady -def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], - unpressed_btn: int = 0) -> List[capnp.lib.capnp._DynamicStructBuilder]: - events: List[capnp.lib.capnp._DynamicStructBuilder] = [] +def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, capnp.lib.capnp._EnumModule], + unpressed_btn: int = 0) -> list[capnp.lib.capnp._DynamicStructBuilder]: + events: list[capnp.lib.capnp._DynamicStructBuilder] = [] if cur_btn == prev_btn: return events @@ -73,7 +77,10 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor return tire_stiffness_front, tire_stiffness_rear -def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]: +DbcDict = dict[str, str] + + +def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> DbcDict: return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} @@ -208,7 +215,7 @@ def get_safety_config(safety_model, safety_param = None): class CanBusBase: offset: int - def __init__(self, CP, fingerprint: Optional[Dict[int, Dict[int, int]]]) -> None: + def __init__(self, CP, fingerprint: dict[int, dict[int, int]] | None) -> None: if CP is None: assert fingerprint is not None num = max([k for k, v in fingerprint.items() if len(v)], default=0) // 4 + 1 @@ -236,3 +243,75 @@ class CanSignalRateCalculator: self.previous_value = current_value return self.rate + + +@dataclass(frozen=True, kw_only=True) +class CarSpecs: + mass: float # kg, curb weight + wheelbase: float # meters + steerRatio: float + centerToFrontRatio: float = 0.5 + minSteerSpeed: float = 0.0 # m/s + minEnableSpeed: float = -1.0 # m/s + tireStiffnessFactor: float = 1.0 + + def override(self, **kwargs): + return replace(self, **kwargs) + + +@dataclass(order=True) +class PlatformConfig(Freezable): + platform_str: str + car_docs: list[CarDocs] + specs: CarSpecs + + dbc_dict: DbcDict + + flags: int = 0 + + def __hash__(self) -> int: + return hash(self.platform_str) + + def override(self, **kwargs): + return replace(self, **kwargs) + + def init(self): + pass + + def __post_init__(self): + self.init() + self.freeze() + + +class Platforms(str, ReprEnum): + config: PlatformConfig + + def __new__(cls, platform_config: PlatformConfig): + member = str.__new__(cls, platform_config.platform_str) + member.config = platform_config + member._value_ = platform_config.platform_str + return member + + @classmethod + def create_dbc_map(cls) -> dict[str, DbcDict]: + return {p: p.config.dbc_dict for p in cls} + + @classmethod + def with_flags(cls, flags: IntFlag) -> set['Platforms']: + return {p for p in cls if p.config.flags & flags} + + @classmethod + def without_flags(cls, flags: IntFlag) -> set['Platforms']: + return {p for p in cls if not (p.config.flags & flags)} + + @classmethod + def print_debug(cls, flags): + platforms_with_flag = defaultdict(list) + for flag in flags: + for platform in cls: + if platform.config.flags & flag: + assert flag.name is not None + platforms_with_flag[flag.name].append(platform) + + for flag, platforms in platforms_with_flag.items(): + print(f"{flag:32s}: {', '.join(p.name for p in platforms)}") diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index abb3f56..6e783e3 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car.body import bodycan from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.pid import PIDController @@ -14,7 +15,7 @@ MAX_POS_INTEGRATOR = 0.2 # meters MAX_TURN_INTEGRATOR = 0.1 # meters -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.frame = 0 self.packer = CANPacker(dbc_name) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 25b39de..fe71b53 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -7,21 +7,17 @@ from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] ret.minSteerSpeed = -math.inf ret.maxLateralAccel = math.inf # TODO: set to a reasonable value - ret.steerRatio = 0.5 ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0. - ret.mass = 9 - ret.wheelbase = 0.406 ret.wheelSpeedFactor = SPEED_FROM_RPM - ret.centerToFront = ret.wheelbase * 0.44 ret.radarUnavailable = True ret.openpilotLongitudinalControl = True diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index 33119bf..d1ba015 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -1,9 +1,6 @@ -from enum import StrEnum -from typing import Dict - from cereal import car -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarInfo +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarDocs from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -22,13 +19,13 @@ class CarControllerParams: pass -class CAR(StrEnum): - BODY = "COMMA BODY" - - -CAR_INFO: Dict[str, CarInfo] = { - CAR.BODY: CarInfo("comma body", package="All"), -} +class CAR(Platforms): + BODY = PlatformConfig( + "COMMA BODY", + [CarDocs("comma body", package="All")], + CarSpecs(mass=9, wheelbase=0.406, steerRatio=0.5, centerToFrontRatio=0.44), + dbc_dict('comma_body', None), + ) FW_QUERY_CONFIG = FwQueryConfig( @@ -41,7 +38,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.BODY: dbc_dict('comma_body', None), -} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 599beb2..a98910c 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -1,9 +1,7 @@ import os -import requests -import sentry_sdk import threading import time -from typing import Callable, Dict, List, Optional, Tuple +from collections.abc import Callable from cereal import car from openpilot.common.params import Params @@ -13,6 +11,7 @@ from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing +from openpilot.selfdrive.car.mock.values import CAR as MOCK from openpilot.common.swaglog import cloudlog import cereal.messaging as messaging import openpilot.selfdrive.sentry as sentry @@ -67,7 +66,7 @@ def load_interfaces(brand_names): return ret -def _get_interface_names() -> Dict[str, List[str]]: +def _get_interface_names() -> dict[str, list[str]]: # returns a dict of brand name and its respective models brand_names = {} for brand_name, brand_models in get_interface_attr("CAR").items(): @@ -81,7 +80,7 @@ interface_names = _get_interface_names() interfaces = load_interfaces(interface_names) -def can_fingerprint(next_can: Callable) -> Tuple[Optional[str], Dict[int, dict]]: +def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 @@ -193,75 +192,18 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached, fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, vin_rx_bus=vin_rx_bus, fingerprints=repr(finger), fw_query_time=fw_query_time, error=True) + return car_fingerprint, finger, vin, car_fw, source, exact_match -def chunk_data(data, size): - return [data[i:i+size] for i in range(0, len(data), size)] -def format_params(params): - return [f"{key}: {value.decode('utf-8') if isinstance(value, bytes) else value}" for key, value in params.items()] +def get_car_interface(CP): + CarInterface, CarController, CarState = interfaces[CP.carFingerprint] + return CarInterface(CP, CarController, CarState) -def get_frogpilot_params(params, keys): - return {key: params.get(key) or '0' for key in keys} -def set_sentry_scope(scope, chunks, label): - scope.set_extra(label, '\n'.join(['\n'.join(chunk) for chunk in chunks])) - -def is_connected_to_internet(timeout=5): - try: - requests.get("https://sentry.io", timeout=timeout) - return True - except Exception: - return False - -def crash_log(params, candidate): - serial_id = params.get("HardwareSerial", encoding='utf-8') - - control_keys, vehicle_keys, visual_keys, tracking_keys = [ - "AdjustablePersonalities", "PersonalitiesViaWheel", "PersonalitiesViaScreen", "AlwaysOnLateral", "AlwaysOnLateralMain", - "ConditionalExperimental", "CESpeed", "CESpeedLead", "CECurves", "CECurvesLead", "CENavigation", "CENavigationIntersections", - "CENavigationLead", "CENavigationTurns", "CESlowerLead", "CEStopLights", "CEStopLightsLead", "CESignal", "CustomPersonalities", - "AggressiveFollow", "AggressiveJerk", "StandardFollow", "StandardJerk", "RelaxedFollow", "RelaxedJerk", "DeviceShutdown", - "ExperimentalModeActivation", "ExperimentalModeViaLKAS", "ExperimentalModeViaScreen", "FireTheBabysitter", "NoLogging", "MuteOverheated", - "OfflineMode", "LateralTune", "ForceAutoTune", "NNFF", "SteerRatio", "UseLateralJerk", "LongitudinalTune", "AccelerationProfile", - "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance", "LeadDetectionThreshold", "SmoothBraking", "Model", "MTSCEnabled", - "DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit", "NudgelessLaneChange", "LaneChangeTime", "LaneDetection", - "LaneDetectionWidth", "OneLaneChange", "QOLControls", "DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", - "ReverseCruiseUI", "SetSpeedLimit", "SetSpeedOffset", "SpeedLimitController", "Offset1", "Offset2", "Offset3", "Offset4", "SLCConfirmation", - "SLCFallback", "SLCPriority1", "SLCPriority2", "SLCPriority3", "SLCOverride", "TurnDesires", "VisionTurnControl", "DisableVTSCSmoothing", - "CurveSensitivity", "TurnAggressiveness" - ], [ - "ForceFingerprint", "DisableOpenpilotLongitudinal", "EVTable", "GasRegenCmd", "LongPitch", "LowerVolt", "CrosstrekTorque", "CydiaTune", - "DragonPilotTune", "FrogsGoMooTune", "LockDoors", "SNGHack" - ], [ - "CustomTheme", "HolidayThemes", "CustomColors", "CustomIcons", "CustomSignals", "CustomSounds", "GoatScream", "AlertVolumeControl", "DisengageVolume", - "EngageVolume", "PromptVolume", "PromptDistractedVolume", "RefuseVolume", "WarningSoftVolume", "WarningImmediateVolume", "CameraView", - "Compass", "CustomAlerts", "GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert", "SpeedLimitChangedAlert", "CustomUI", "AccelerationPath", - "AdjacentPath", "AdjacentPathMetrics", "BlindSpotPath", "FPSCounter", "LeadInfo", "UseSI", "PedalsOnUI", "RoadNameUI", "UseVienna", "DriverCamera", - "ModelUI", "DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength", "QOLVisuals", "DriveStats", - "FullMap", "HideSpeed", "HideSpeedUI", "ShowSLCOffset", "SpeedLimitChangedAlert", "WheelSpeed", "RandomEvents", "ScreenBrightness", "WheelIcon", - "RotatingWheel", "NumericalTemp", "Fahrenheit", "ShowCPU", "ShowGPU", "ShowIP", "ShowMemoryUsage", "ShowStorageLeft", "ShowStorageUsed", "Sidebar" - ], [ - "FrogPilotDrives", "FrogPilotKilometers", "FrogPilotMinutes" - ] - - control_params, vehicle_params, visual_params, tracking_params = map(lambda keys: get_frogpilot_params(params, keys), [control_keys, vehicle_keys, visual_keys, tracking_keys]) - control_values, vehicle_values, visual_values, tracking_values = map(format_params, [control_params, vehicle_params, visual_params, tracking_params]) - control_chunks, vehicle_chunks, visual_chunks, tracking_chunks = map(lambda data: chunk_data(data, 50), [control_values, vehicle_values, visual_values, tracking_values]) - - while not is_connected_to_internet(): - time.sleep(60) - - with sentry_sdk.configure_scope() as scope: - for chunks, label in zip([control_chunks, vehicle_chunks, visual_chunks, tracking_chunks], ["FrogPilot Controls", "FrogPilot Vehicles", "FrogPilot Visuals", "FrogPilot Tracking"]): - set_sentry_scope(scope, chunks, label) - sentry.capture_warning(f"Fingerprinted: {candidate}", serial_id) - -def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): - params = Params() +def get_car(params, logcan, sendcan, disable_openpilot_long, experimental_long_allowed, num_pandas=1): car_brand = params.get("CarMake", encoding='utf-8') car_model = params.get("CarModel", encoding='utf-8') - dongle_id = params.get("DongleId", encoding='utf-8') force_fingerprint = params.get_bool("ForceFingerprint") @@ -273,33 +215,37 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): else: cloudlog.event("car doesn't match any fingerprints", fingerprints=repr(fingerprints), error=True) candidate = "mock" - elif car_model is None: + + if car_model is None and candidate != "mock": params.put("CarMake", candidate.split(' ')[0].title()) params.put("CarModel", candidate) - if get_short_branch() == "FrogPilot-Development" and not Params("/persist/comma/params").get_bool("FrogsGoMoo"): + if get_short_branch() == "FrogPilot-Development" and not Params("/persist/params").get_bool("FrogsGoMoo"): + cloudlog.event("Blocked user from using the 'FrogPilot-Development' branch", fingerprints=repr(fingerprints), error=True) candidate = "mock" + fingerprint_log = threading.Thread(target=sentry.capture_fingerprint, args=(params, candidate, True,)) + fingerprint_log.start() + elif not params.get_bool("FingerprintLogged"): + fingerprint_log = threading.Thread(target=sentry.capture_fingerprint, args=(params, candidate,)) + fingerprint_log.start() - setFingerprintLog = threading.Thread(target=crash_log, args=(params, candidate,)) - setFingerprintLog.start() - - CarInterface, CarController, CarState = interfaces[candidate] - CP = CarInterface.get_params(params, candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) + CarInterface, _, _ = interfaces[candidate] + CP = CarInterface.get_params(params, candidate, fingerprints, car_fw, disable_openpilot_long, experimental_long_allowed, docs=False) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source CP.fuzzyFingerprint = not exact_match - return CarInterface(CP, CarController, CarState), CP + return get_car_interface(CP), CP -def write_car_param(fingerprint="mock"): +def write_car_param(platform=MOCK.MOCK): params = Params() - CarInterface, _, _ = interfaces[fingerprint] - CP = CarInterface.get_non_essential_params(fingerprint) + CarInterface, _, _ = interfaces[platform] + CP = CarInterface.get_non_essential_params(platform) params.put("CarParams", CP.to_bytes()) def get_demo_car_params(): - fingerprint="mock" - CarInterface, _, _ = interfaces[fingerprint] - CP = CarInterface.get_non_essential_params(fingerprint) + platform = MOCK.MOCK + CarInterface, _, _ = interfaces[platform] + CP = CarInterface.get_non_essential_params(platform) return CP diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py new file mode 100755 index 0000000..82ae811 --- /dev/null +++ b/selfdrive/car/card.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +import os +import time + +import cereal.messaging as messaging + +from cereal import car + +from panda import ALTERNATIVE_EXPERIENCE + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_CTRL + +from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp +from openpilot.selfdrive.car.car_helpers import get_car, get_one_can +from openpilot.selfdrive.car.interfaces import CarInterfaceBase + + +REPLAY = "REPLAY" in os.environ + + +class CarD: + CI: CarInterfaceBase + CS: car.CarState + + def __init__(self, CI=None): + self.can_sock = messaging.sub_sock('can', timeout=20) + self.sm = messaging.SubMaster(['pandaStates']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) + + self.can_rcv_timeout_counter = 0 # conseuctive timeout count + self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count + + self.CC_prev = car.CarControl.new_message() + + self.last_actuators = None + + self.params = Params() + + if CI is None: + # wait for one pandaState and one CAN packet + print("Waiting for CAN messages...") + get_one_can(self.can_sock) + + num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) + disable_openpilot_long = self.params.get_bool("DisableOpenpilotLongitudinal") + experimental_long_allowed = not disable_openpilot_long and self.params.get_bool("ExperimentalLongitudinalEnabled") + self.CI, self.CP = get_car(self.params, self.can_sock, self.pm.sock['sendcan'], disable_openpilot_long, experimental_long_allowed, num_pandas) + else: + self.CI, self.CP = CI, CI.CP + + # set alternative experiences from parameters + disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.CP.alternativeExperience = 0 + if not disengage_on_accelerator: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + + always_on_lateral = self.params.get_bool("AlwaysOnLateral") + if always_on_lateral: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX + + car_recognized = self.CP.carName != 'mock' + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") + + controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly + + self.CP.passive = not car_recognized or not controller_available or self.CP.dashcamOnly + if self.CP.passive: + safety_config = car.CarParams.SafetyConfig.new_message() + safety_config.safetyModel = car.CarParams.SafetyModel.noOutput + self.CP.safetyConfigs = [safety_config] + + # Write previous route's CarParams + prev_cp = self.params.get("CarParamsPersistent") + if prev_cp is not None: + self.params.put("CarParamsPrevRoute", prev_cp) + + # Write CarParams for controls and radard + cp_bytes = self.CP.to_bytes() + self.params.put("CarParams", cp_bytes) + self.params.put_nonblocking("CarParamsCache", cp_bytes) + self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + + def initialize(self): + """Initialize CarInterface, once controls are ready""" + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + + def state_update(self, frogpilot_variables): + """carState update loop, driven by can""" + + # Update carState from CAN + can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) + self.CS = self.CI.update(self.CC_prev, can_strs, frogpilot_variables) + + self.sm.update(0) + + can_rcv_valid = len(can_strs) > 0 + + # Check for CAN timeout + if not can_rcv_valid: + self.can_rcv_timeout_counter += 1 + self.can_rcv_cum_timeout_counter += 1 + else: + self.can_rcv_timeout_counter = 0 + + self.can_rcv_timeout = self.can_rcv_timeout_counter >= 5 + + if can_rcv_valid and REPLAY: + self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + + self.state_publish() + + return self.CS + + def state_publish(self): + """carState and carParams publish loop""" + + # carState + cs_send = messaging.new_message('carState') + cs_send.valid = self.CS.canValid + cs_send.carState = self.CS + self.pm.send('carState', cs_send) + + # carParams - logged every 50 seconds (> 1 per segment) + if (self.sm.frame % int(50. / DT_CTRL) == 0): + cp_send = messaging.new_message('carParams') + cp_send.valid = True + cp_send.carParams = self.CP + self.pm.send('carParams', cp_send) + + # publish new carOutput + co_send = messaging.new_message('carOutput') + co_send.valid = True + if self.last_actuators is not None: + co_send.carOutput.actuatorsOutput = self.last_actuators + self.pm.send('carOutput', co_send) + + def controls_update(self, CC: car.CarControl, frogpilot_variables): + """control update loop, driven by carControl""" + + # send car controls over can + now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) + self.last_actuators, can_sends = self.CI.apply(CC, now_nanos, frogpilot_variables) + self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid)) + + self.CC_prev = CC diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 820c761..8c72b76 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -3,9 +3,10 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_meas_steer_torque_limits from openpilot.selfdrive.car.chrysler import chryslercan from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags +from openpilot.selfdrive.car.interfaces import CarControllerBase -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 56a4dbc..9c5feda 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -21,10 +21,16 @@ class CarState(CarStateBase): else: self.shifter_values = can_define.dv["GEAR"]["PRNDL"] + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["CRUISE_BUTTONS"]["ACC_Distance_Dec"] + # lock info ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], cp.vl["BCM_1"]["DOOR_OPEN_FR"], @@ -95,6 +101,12 @@ class CarState(CarStateBase): self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] + self.lkas_previously_enabled = self.lkas_enabled + if self.CP.carFingerprint in RAM_CARS: + self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"] + else: + self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1 + return ret @staticmethod diff --git a/selfdrive/car/chrysler/fingerprints.py b/selfdrive/car/chrysler/fingerprints.py index 1df514e..219ec3e 100644 --- a/selfdrive/car/chrysler/fingerprints.py +++ b/selfdrive/car/chrysler/fingerprints.py @@ -38,6 +38,7 @@ FW_VERSIONS = { b'68227902AF', b'68227902AG', b'68227902AH', + b'68227905AG', b'68360252AC', ], (Ecu.srs, 0x744, None): [ @@ -71,6 +72,7 @@ FW_VERSIONS = { b'68340762AD ', b'68340764AD ', b'68352652AE ', + b'68352654AE ', b'68366851AH ', b'68366853AE ', b'68372861AF ', @@ -93,6 +95,7 @@ FW_VERSIONS = { b'68405327AC', b'68436233AB', b'68436233AC', + b'68436234AB', b'68436250AE', b'68529067AA', b'68594993AB', @@ -304,6 +307,7 @@ FW_VERSIONS = { b'68402708AB', b'68402971AD', b'68454144AD', + b'68454145AB', b'68454152AB', b'68454156AB', b'68516650AB', @@ -376,6 +380,7 @@ FW_VERSIONS = { b'68434859AC', b'68434860AC', b'68453483AC', + b'68453483AD', b'68453487AD', b'68453491AC', b'68453499AD', @@ -401,6 +406,7 @@ FW_VERSIONS = { b'68527383AD', b'68527387AE', b'68527403AC', + b'68527403AD', b'68546047AF', b'68631938AA', b'68631942AA', @@ -474,6 +480,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'05035699AG ', + b'05035841AC ', b'05036026AB ', b'05036065AE ', b'05036066AE ', @@ -506,11 +513,13 @@ FW_VERSIONS = { b'68455145AE ', b'68455146AC ', b'68467915AC ', + b'68467916AC ', b'68467936AC ', b'68500630AD', b'68500630AE', b'68502719AC ', b'68502722AC ', + b'68502733AC ', b'68502734AF ', b'68502740AF ', b'68502741AF ', diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 8278e55..d45efc6 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,14 +1,17 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from panda import Panda -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type + class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD @@ -24,7 +27,6 @@ class CarInterface(CarInterfaceBase): elif candidate in RAM_DT: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT - ret.minSteerSpeed = 3.8 # m/s CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) if candidate not in RAM_CARS: # Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed. @@ -35,10 +37,6 @@ class CarInterface(CarInterfaceBase): # Chrysler if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.DODGE_DURANGO): - ret.mass = 2242. - ret.wheelbase = 3.089 - ret.steerRatio = 16.2 # Pacifica Hybrid 2017 - ret.lateralTuning.init('pid') ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] @@ -46,9 +44,6 @@ class CarInterface(CarInterfaceBase): # Jeep elif candidate in (CAR.JEEP_GRAND_CHEROKEE, CAR.JEEP_GRAND_CHEROKEE_2019): - ret.mass = 1778 - ret.wheelbase = 2.71 - ret.steerRatio = 16.7 ret.steerActuatorDelay = 0.2 ret.lateralTuning.init('pid') @@ -60,19 +55,12 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.RAM_1500: ret.steerActuatorDelay = 0.2 ret.wheelbase = 3.88 - ret.steerRatio = 16.3 - ret.mass = 2493. - ret.minSteerSpeed = 14.5 # Older EPS FW allow steer to zero if any(fw.ecu == 'eps' and b"68" < fw.fwVersion[:4] <= b"6831" for fw in car_fw): ret.minSteerSpeed = 0. elif candidate == CAR.RAM_HD: ret.steerActuatorDelay = 0.2 - ret.wheelbase = 3.785 - ret.steerRatio = 15.61 - ret.mass = 3405. - ret.minSteerSpeed = 16 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False) else: @@ -91,8 +79,13 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + # events - events = self.create_common_events(ret, frogpilot_variables, extra_gears=[car.CarState.GearShifter.low]) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5): diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 94ff1f1..dfda1d1 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,38 +1,102 @@ -from enum import IntFlag, StrEnum +from enum import IntFlag from dataclasses import dataclass, field -from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu class ChryslerFlags(IntFlag): + # Detected flags HIGHER_MIN_STEERING_SPEED = 1 +@dataclass +class ChryslerCarDocs(CarDocs): + package: str = "Adaptive Cruise Control (ACC)" + car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca])) -class CAR(StrEnum): + +@dataclass +class ChryslerPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion')) + + +@dataclass(frozen=True) +class ChryslerCarSpecs(CarSpecs): + minSteerSpeed: float = 3.8 # m/s + + +class CAR(Platforms): # Chrysler - PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" - PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" - PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" - PACIFICA_2018 = "CHRYSLER PACIFICA 2018" - PACIFICA_2020 = "CHRYSLER PACIFICA 2020" + PACIFICA_2017_HYBRID = ChryslerPlatformConfig( + "CHRYSLER PACIFICA HYBRID 2017", + [ChryslerCarDocs("Chrysler Pacifica Hybrid 2017")], + ChryslerCarSpecs(mass=2242., wheelbase=3.089, steerRatio=16.2), + ) + PACIFICA_2018_HYBRID = ChryslerPlatformConfig( + "CHRYSLER PACIFICA HYBRID 2018", + [ChryslerCarDocs("Chrysler Pacifica Hybrid 2018")], + PACIFICA_2017_HYBRID.specs, + ) + PACIFICA_2019_HYBRID = ChryslerPlatformConfig( + "CHRYSLER PACIFICA HYBRID 2019", + [ChryslerCarDocs("Chrysler Pacifica Hybrid 2019-23")], + PACIFICA_2017_HYBRID.specs, + ) + PACIFICA_2018 = ChryslerPlatformConfig( + "CHRYSLER PACIFICA 2018", + [ChryslerCarDocs("Chrysler Pacifica 2017-18")], + PACIFICA_2017_HYBRID.specs, + ) + PACIFICA_2020 = ChryslerPlatformConfig( + "CHRYSLER PACIFICA 2020", + [ + ChryslerCarDocs("Chrysler Pacifica 2019-20"), + ChryslerCarDocs("Chrysler Pacifica 2021-23", package="All"), + ], + PACIFICA_2017_HYBRID.specs, + ) # Dodge - DODGE_DURANGO = "DODGE DURANGO 2021" + DODGE_DURANGO = ChryslerPlatformConfig( + "DODGE DURANGO 2021", + [ChryslerCarDocs("Dodge Durango 2020-21")], + PACIFICA_2017_HYBRID.specs, + ) # Jeep - JEEP_GRAND_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk - JEEP_GRAND_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk + JEEP_GRAND_CHEROKEE = ChryslerPlatformConfig( # includes 2017 Trailhawk + "JEEP GRAND CHEROKEE V6 2018", + [ChryslerCarDocs("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk")], + ChryslerCarSpecs(mass=1778., wheelbase=2.71, steerRatio=16.7), + ) + + JEEP_GRAND_CHEROKEE_2019 = ChryslerPlatformConfig( # includes 2020 Trailhawk + "JEEP GRAND CHEROKEE 2019", + [ChryslerCarDocs("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4")], + JEEP_GRAND_CHEROKEE.specs, + ) # Ram - RAM_1500 = "RAM 1500 5TH GEN" - RAM_HD = "RAM HD 5TH GEN" + RAM_1500 = ChryslerPlatformConfig( + "RAM 1500 5TH GEN", + [ChryslerCarDocs("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram]))], + ChryslerCarSpecs(mass=2493., wheelbase=3.88, steerRatio=16.3, minSteerSpeed=14.5), + dbc_dict('chrysler_ram_dt_generated', None), + ) + RAM_HD = ChryslerPlatformConfig( + "RAM HD 5TH GEN", + [ + ChryslerCarDocs("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])), + ChryslerCarDocs("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])), + ], + ChryslerCarSpecs(mass=3405., wheelbase=3.785, steerRatio=15.61, minSteerSpeed=16.), + dbc_dict('chrysler_ram_hd_generated', None), + ) class CarControllerParams: @@ -60,32 +124,6 @@ RAM_HD = {CAR.RAM_HD, } RAM_CARS = RAM_DT | RAM_HD -@dataclass -class ChryslerCarInfo(CarInfo): - package: str = "Adaptive Cruise Control (ACC)" - car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca])) - - -CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { - CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017"), - CAR.PACIFICA_2018_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2018"), - CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-23"), - CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), - CAR.PACIFICA_2020: [ - ChryslerCarInfo("Chrysler Pacifica 2019-20"), - ChryslerCarInfo("Chrysler Pacifica 2021-23", package="All"), - ], - CAR.JEEP_GRAND_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), - CAR.JEEP_GRAND_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), - CAR.DODGE_DURANGO: ChryslerCarInfo("Dodge Durango 2020-21"), - CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram])), - CAR.RAM_HD: [ - ChryslerCarInfo("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])), - ChryslerCarInfo("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])), - ], -} - - CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(0xf132) CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ @@ -125,16 +163,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.DODGE_DURANGO: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.JEEP_GRAND_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.JEEP_GRAND_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.RAM_1500: dbc_dict('chrysler_ram_dt_generated', None), - CAR.RAM_HD: dbc_dict('chrysler_ram_hd_generated', None), -} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py new file mode 100644 index 0000000..7bf6a6a --- /dev/null +++ b/selfdrive/car/docs.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +import argparse +from collections import defaultdict +import jinja2 +import os +from enum import Enum +from natsort import natsorted + +from cereal import car +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.car import gen_empty_fingerprint +from openpilot.selfdrive.car.docs_definitions import CarDocs, Column, CommonFootnote, PartType +from openpilot.selfdrive.car.car_helpers import interfaces, get_interface_attr +from openpilot.selfdrive.car.values import PLATFORMS + + +def get_all_footnotes() -> dict[Enum, int]: + all_footnotes = list(CommonFootnote) + for footnotes in get_interface_attr("Footnote", ignore_none=True).values(): + all_footnotes.extend(footnotes) + return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)} + + +CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md") +CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") + + +def get_all_car_docs() -> list[CarDocs]: + all_car_docs: list[CarDocs] = [] + footnotes = get_all_footnotes() + for model, platform in PLATFORMS.items(): + car_docs = platform.config.car_docs + # If available, uses experimental longitudinal limits for the docs + CP = interfaces[model][0].get_params(platform, fingerprint=gen_empty_fingerprint(), + car_fw=[car.CarParams.CarFw(ecu="unknown")], experimental_long=True, docs=True) + + if CP.dashcamOnly or not len(car_docs): + continue + + # A platform can include multiple car models + for _car_docs in car_docs: + if not hasattr(_car_docs, "row"): + _car_docs.init_make(CP) + _car_docs.init(CP, footnotes) + all_car_docs.append(_car_docs) + + # Sort cars by make and model + year + sorted_cars: list[CarDocs] = natsorted(all_car_docs, key=lambda car: car.name.lower()) + return sorted_cars + + +def group_by_make(all_car_docs: list[CarDocs]) -> dict[str, list[CarDocs]]: + sorted_car_docs = defaultdict(list) + for car_docs in all_car_docs: + sorted_car_docs[car_docs.make].append(car_docs) + return dict(sorted_car_docs) + + +def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str) -> str: + with open(template_fn) as f: + template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True) + + footnotes = [fn.value.text for fn in get_all_footnotes()] + cars_md: str = template.render(all_car_docs=all_car_docs, PartType=PartType, + group_by_make=group_by_make, footnotes=footnotes, + Column=Column) + return cars_md + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Auto generates supported cars documentation", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("--template", default=CARS_MD_TEMPLATE, help="Override default template filename") + parser.add_argument("--out", default=CARS_MD_OUT, help="Override default generated filename") + args = parser.parse_args() + + with open(args.out, 'w') as f: + f.write(generate_cars_md(get_all_car_docs(), args.template)) + print(f"Generated and written to {args.out}") diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index f2ce743..c9d8dd1 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -3,7 +3,6 @@ from collections import namedtuple import copy from dataclasses import dataclass, field from enum import Enum -from typing import Dict, List, Optional, Tuple, Union from cereal import car from openpilot.common.conversions import Conversions as CV @@ -35,7 +34,7 @@ class Star(Enum): @dataclass class BasePart: name: str - parts: List[Enum] = field(default_factory=list) + parts: list[Enum] = field(default_factory=list) def all_parts(self): # Recursively get all parts @@ -76,7 +75,7 @@ class Accessory(EnumBase): @dataclass class BaseCarHarness(BasePart): - parts: List[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft]) + parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft]) has_connector: bool = True # without are hidden on the harness connector page @@ -119,7 +118,8 @@ class CarHarness(EnumBase): nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler]) mazda = BaseCarHarness("Mazda connector") ford_q3 = BaseCarHarness("Ford Q3 connector") - ford_q4 = BaseCarHarness("Ford Q4 connector") + ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft, Cable.long_obdc_cable, + Cable.usbc_coupler]) class Device(EnumBase): @@ -149,18 +149,18 @@ class PartType(Enum): tool = Tool -DEFAULT_CAR_PARTS: List[EnumBase] = [Device.threex] +DEFAULT_CAR_PARTS: list[EnumBase] = [Device.threex] @dataclass class CarParts: - parts: List[EnumBase] = field(default_factory=list) + parts: list[EnumBase] = field(default_factory=list) def __call__(self): return copy.deepcopy(self) @classmethod - def common(cls, add: Optional[List[EnumBase]] = None, remove: Optional[List[EnumBase]] = None): + def common(cls, add: list[EnumBase] = None, remove: list[EnumBase] = None): p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])] return cls(p) @@ -186,7 +186,7 @@ class CommonFootnote(Enum): Column.LONGITUDINAL) -def get_footnotes(footnotes: List[Enum], column: Column) -> List[Enum]: +def get_footnotes(footnotes: list[Enum], column: Column) -> list[Enum]: # Returns applicable footnotes given current column return [fn for fn in footnotes if fn.value.column == column] @@ -209,7 +209,7 @@ def get_year_list(years): return years_list -def split_name(name: str) -> Tuple[str, str, str]: +def split_name(name: str) -> tuple[str, str, str]: make, model = name.split(" ", 1) years = "" match = re.search(MODEL_YEARS_RE, model) @@ -220,7 +220,7 @@ def split_name(name: str) -> Tuple[str, str, str]: @dataclass -class CarInfo: +class CarDocs: # make + model + model years name: str @@ -233,13 +233,13 @@ class CarInfo: # the minimum compatibility requirements for this model, regardless # of market. can be a package, trim, or list of features - requirements: Optional[str] = None + requirements: str | None = None - video_link: Optional[str] = None - footnotes: List[Enum] = field(default_factory=list) - min_steer_speed: Optional[float] = None - min_enable_speed: Optional[float] = None - auto_resume: Optional[bool] = None + video_link: str | None = None + footnotes: list[Enum] = field(default_factory=list) + min_steer_speed: float | None = None + min_enable_speed: float | None = None + auto_resume: bool | None = None # all the parts needed for the supported car car_parts: CarParts = field(default_factory=CarParts) @@ -248,7 +248,7 @@ class CarInfo: self.make, self.model, self.years = split_name(self.name) self.year_list = get_year_list(self.years) - def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): + def init(self, CP: car.CarParams, all_footnotes: dict[Enum, int]): self.car_name = CP.carName self.car_fingerprint = CP.carFingerprint @@ -266,7 +266,7 @@ class CarInfo: # min steer & enable speed columns # TODO: set all the min steer speeds in carParams and remove this if self.min_steer_speed is not None: - assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams" + assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarDocs and CarParams" else: self.min_steer_speed = CP.minSteerSpeed @@ -293,7 +293,7 @@ class CarInfo: if len(tools_docs): hardware_col += f'
Tools{display_func(tools_docs)}
' - self.row: Dict[Enum, Union[str, Star]] = { + self.row: dict[Enum, str | Star] = { Column.MAKE: self.make, Column.MODEL: self.model, Column.PACKAGE: self.package, @@ -317,7 +317,7 @@ class CarInfo: return self def init_make(self, CP: car.CarParams): - """CarInfo subclasses can add make-specific logic for harness selection, footnotes, etc.""" + """CarDocs subclasses can add make-specific logic for harness selection, footnotes, etc.""" def get_detail_sentence(self, CP): if not CP.notCar: @@ -352,7 +352,7 @@ class CarInfo: raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}") def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str: - item: Union[str, Star] = self.row[column] + item: str | Star = self.row[column] if isinstance(item, Star): item = star_icon.format(item.value) elif column == Column.MODEL and len(self.years): diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 13f7926..da5e7b4 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import capnp import time -from typing import Optional, Set import cereal.messaging as messaging from panda.python.uds import SERVICE_TYPE @@ -20,7 +19,7 @@ def make_tester_present_msg(addr, bus, subaddr=None): return make_can_msg(addr, bytes(dat), bus) -def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: Optional[int] = None) -> bool: +def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: int = None) -> bool: # ISO-TP messages are always padded to 8 bytes # tester present response is always a single frame dat_offset = 1 if subaddr is not None else 0 @@ -34,16 +33,16 @@ def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subadd return False -def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[EcuAddrBusType]: +def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> set[EcuAddrBusType]: addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)] - queries: Set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list} + queries: set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list} responses = queries return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) -def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[EcuAddrBusType], - responses: Set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> Set[EcuAddrBusType]: - ecu_responses: Set[EcuAddrBusType] = set() # set((addr, subaddr, bus),) +def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: set[EcuAddrBusType], + responses: set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> set[EcuAddrBusType]: + ecu_responses: set[EcuAddrBusType] = set() # set((addr, subaddr, bus),) try: msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries] diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py index 6a7c3c7..eaf9002 100644 --- a/selfdrive/car/fingerprints.py +++ b/selfdrive/car/fingerprints.py @@ -1,4 +1,8 @@ from openpilot.selfdrive.car.interfaces import get_interface_attr +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.volkswagen.values import CAR as VW FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True) _FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True) @@ -44,3 +48,73 @@ def all_known_cars(): def all_legacy_fingerprint_cars(): """Returns a list of all known car strings, FPv1 only.""" return list(_FINGERPRINTS.keys()) + + +# A dict that maps old platform strings to their latest representations +MIGRATION = { + "ACURA ILX 2016 ACURAWATCH PLUS": HONDA.ACURA_ILX, + "ACURA RDX 2018 ACURAWATCH PLUS": HONDA.ACURA_RDX, + "ACURA RDX 2020 TECH": HONDA.ACURA_RDX_3G, + "AUDI A3": VW.AUDI_A3_MK3, + "HONDA ACCORD 2018 HYBRID TOURING": HONDA.ACCORD, + "HONDA ACCORD 1.5T 2018": HONDA.ACCORD, + "HONDA ACCORD 2018 LX 1.5T": HONDA.ACCORD, + "HONDA ACCORD 2018 SPORT 2T": HONDA.ACCORD, + "HONDA ACCORD 2T 2018": HONDA.ACCORD, + "HONDA ACCORD HYBRID 2018": HONDA.ACCORD, + "HONDA CIVIC 2016 TOURING": HONDA.CIVIC, + "HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019": HONDA.CIVIC_BOSCH, + "HONDA CIVIC SEDAN 1.6 DIESEL": HONDA.CIVIC_BOSCH_DIESEL, + "HONDA CR-V 2016 EXECUTIVE": HONDA.CRV_EU, + "HONDA CR-V 2016 TOURING": HONDA.CRV, + "HONDA CR-V 2017 EX": HONDA.CRV_5G, + "HONDA CR-V 2019 HYBRID": HONDA.CRV_HYBRID, + "HONDA FIT 2018 EX": HONDA.FIT, + "HONDA HRV 2019 TOURING": HONDA.HRV, + "HONDA INSIGHT 2019 TOURING": HONDA.INSIGHT, + "HONDA ODYSSEY 2018 EX-L": HONDA.ODYSSEY, + "HONDA ODYSSEY 2019 EXCLUSIVE CHN": HONDA.ODYSSEY_CHN, + "HONDA PILOT 2017 TOURING": HONDA.PILOT, + "HONDA PILOT 2019 ELITE": HONDA.PILOT, + "HONDA PILOT 2019": HONDA.PILOT, + "HONDA PASSPORT 2021": HONDA.PILOT, + "HONDA RIDGELINE 2017 BLACK EDITION": HONDA.RIDGELINE, + "HYUNDAI ELANTRA LIMITED ULTIMATE 2017": HYUNDAI.ELANTRA, + "HYUNDAI SANTA FE LIMITED 2019": HYUNDAI.SANTA_FE, + "HYUNDAI TUCSON DIESEL 2019": HYUNDAI.TUCSON, + "KIA OPTIMA 2016": HYUNDAI.KIA_OPTIMA_G4, + "KIA OPTIMA 2019": HYUNDAI.KIA_OPTIMA_G4_FL, + "KIA OPTIMA SX 2019 & 2016": HYUNDAI.KIA_OPTIMA_G4_FL, + "LEXUS CT 200H 2018": TOYOTA.LEXUS_CTH, + "LEXUS ES 300H 2018": TOYOTA.LEXUS_ES, + "LEXUS ES 300H 2019": TOYOTA.LEXUS_ES_TSS2, + "LEXUS IS300 2018": TOYOTA.LEXUS_IS, + "LEXUS NX300 2018": TOYOTA.LEXUS_NX, + "LEXUS NX300H 2018": TOYOTA.LEXUS_NX, + "LEXUS RX 350 2016": TOYOTA.LEXUS_RX, + "LEXUS RX350 2020": TOYOTA.LEXUS_RX_TSS2, + "LEXUS RX450 HYBRID 2020": TOYOTA.LEXUS_RX_TSS2, + "TOYOTA SIENNA XLE 2018": TOYOTA.SIENNA, + "TOYOTA C-HR HYBRID 2018": TOYOTA.CHR, + "TOYOTA COROLLA HYBRID TSS2 2019": TOYOTA.COROLLA_TSS2, + "TOYOTA RAV4 HYBRID 2019": TOYOTA.RAV4_TSS2, + "LEXUS ES HYBRID 2019": TOYOTA.LEXUS_ES_TSS2, + "LEXUS NX HYBRID 2018": TOYOTA.LEXUS_NX, + "LEXUS NX HYBRID 2020": TOYOTA.LEXUS_NX_TSS2, + "LEXUS RX HYBRID 2020": TOYOTA.LEXUS_RX_TSS2, + "TOYOTA ALPHARD HYBRID 2021": TOYOTA.ALPHARD_TSS2, + "TOYOTA AVALON HYBRID 2019": TOYOTA.AVALON_2019, + "TOYOTA AVALON HYBRID 2022": TOYOTA.AVALON_TSS2, + "TOYOTA CAMRY HYBRID 2018": TOYOTA.CAMRY, + "TOYOTA CAMRY HYBRID 2021": TOYOTA.CAMRY_TSS2, + "TOYOTA C-HR HYBRID 2022": TOYOTA.CHR_TSS2, + "TOYOTA HIGHLANDER HYBRID 2020": TOYOTA.HIGHLANDER_TSS2, + "TOYOTA RAV4 HYBRID 2022": TOYOTA.RAV4_TSS2_2022, + "TOYOTA RAV4 HYBRID 2023": TOYOTA.RAV4_TSS2_2023, + "TOYOTA HIGHLANDER HYBRID 2018": TOYOTA.HIGHLANDER, + "LEXUS ES HYBRID 2018": TOYOTA.LEXUS_ES, + "LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX, + "HYUNDAI TUCSON HYBRID 4TH GEN": HYUNDAI.TUCSON_4TH_GEN, + "KIA SPORTAGE HYBRID 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, + "KIA SORENTO PLUG-IN HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, +} diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 4b7a992..dabc454 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,9 +1,10 @@ from cereal import car -from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker +from openpilot.common.numpy_fast import clip from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams +from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState @@ -22,7 +23,7 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.VM = VM @@ -34,6 +35,7 @@ class CarController: self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False + self.lead_distance_bars_last = None def update(self, CC, CS, now_nanos, frogpilot_variables): can_sends = [] @@ -69,10 +71,10 @@ class CarController: self.apply_curvature_last = apply_curvature - if self.CP.carFingerprint in CANFD_CAR: + if self.CP.flags & FordFlags.CANFD: # TODO: extended mode mode = 1 if CC.latActive else 0 - counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF + counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) else: can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.)) @@ -100,15 +102,19 @@ class CarController: # send lkas ui msg at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + # send acc ui msg at 5Hz or if ui state changes + if hud_control.leadDistanceBars != self.lead_distance_bars_last: + send_ui = True if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, - fcw_alert, CS.out.cruiseState.standstill, hud_control, - CS.acc_tja_status_stock_values)) + fcw_alert, CS.out.cruiseState.standstill, hud_control, + CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert + self.lead_distance_bars_last = hud_control.leadDistanceBars new_actuators = actuators.copy() new_actuators.curvature = self.apply_curvature_last diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index da8f635..60cee68 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,10 +1,10 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.interfaces import CarStateBase +from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC +from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags +from openpilot.selfdrive.car.interfaces import CarStateBase GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -18,17 +18,13 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] self.vehicle_sensors_valid = False - self.unsupported_platform = False + + self.prev_distance_button = 0 + self.distance_button = 0 def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() - # Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums, - # this must be root-caused before enabling support. Ford Q4 hybrids do not have this problem. - # TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants - self.unsupported_platform = (cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 and - self.CP.carFingerprint not in CANFD_CAR) - # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement # The vehicle usually recovers out of this state within a minute of normal driving self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3 @@ -56,7 +52,7 @@ class CarState(CarStateBase): ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0 # 0 is default mode - if self.CP.carFingerprint in CANFD_CAR: + if self.CP.flags & FordFlags.CANFD: # this signal is always 0 on non-CAN FD cars ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3) @@ -90,6 +86,8 @@ class CarState(CarStateBase): ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 # TODO: block this going to the camera otherwise it will enable stock TJA ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"] # lock info ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"], @@ -98,7 +96,7 @@ class CarState(CarStateBase): # blindspot sensors if self.CP.enableBsm: - cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp + cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD else cp ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 @@ -108,6 +106,9 @@ class CarState(CarStateBase): self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) + return ret @staticmethod @@ -129,7 +130,7 @@ class CarState(CarStateBase): ("RCMStatusMessage2_FD1", 10), ] - if CP.carFingerprint in CANFD_CAR: + if CP.flags & FordFlags.CANFD: messages += [ ("Lane_Assist_Data3_FD1", 33), ] @@ -144,7 +145,7 @@ class CarState(CarStateBase): ("BCM_Lamp_Stat_FD1", 1), ] - if CP.enableBsm and CP.carFingerprint not in CANFD_CAR: + if CP.enableBsm and not (CP.flags & FordFlags.CANFD): messages += [ ("Side_Detect_L_Stat", 5), ("Side_Detect_R_Stat", 5), @@ -162,7 +163,7 @@ class CarState(CarStateBase): ("IPMA_Data", 1), ] - if CP.enableBsm and CP.carFingerprint in CANFD_CAR: + if CP.enableBsm and CP.flags & FordFlags.CANFD: messages += [ ("Side_Detect_L_Stat", 5), ("Side_Detect_R_Stat", 5), diff --git a/selfdrive/car/ford/fingerprints.py b/selfdrive/car/ford/fingerprints.py index a5d4658..fae529a 100644 --- a/selfdrive/car/ford/fingerprints.py +++ b/selfdrive/car/ford/fingerprints.py @@ -8,16 +8,19 @@ FW_VERSIONS = { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-RF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.ESCAPE_MK4: { @@ -82,6 +85,7 @@ FW_VERSIONS = { b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ + b'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, @@ -133,6 +137,7 @@ FW_VERSIONS = { b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ6C-2D053-EF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index c5ef090..6c93a52 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -212,7 +212,7 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator "AccStopMde_B_Dsply": 1 if standstill else 0, "AccWarn_D_Dsply": 0, # ACC warning - "AccTGap_D_Dsply": 4, # Fixed time gap in UI + "AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap }) # Forwards FCW alert from IPMA diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 9fc4d82..e4374ee 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,18 +1,20 @@ -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu +from openpilot.selfdrive.car.ford.values import Ecu, FordFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type TransmissionType = car.CarParams.TransmissionType GearShifter = car.CarState.GearShifter +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "ford" ret.dashcamOnly = False @@ -34,56 +36,11 @@ class CarInterface(CarInterfaceBase): ret.experimentalLongitudinalAvailable = True if experimental_long: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = True - if candidate in CANFD_CAR: + if ret.flags & FordFlags.CANFD: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD - if candidate == CAR.BRONCO_SPORT_MK1: - ret.wheelbase = 2.67 - ret.steerRatio = 17.7 - ret.mass = 1625 - - elif candidate == CAR.ESCAPE_MK4: - ret.wheelbase = 2.71 - ret.steerRatio = 16.7 - ret.mass = 1750 - - elif candidate == CAR.EXPLORER_MK6: - ret.wheelbase = 3.025 - ret.steerRatio = 16.8 - ret.mass = 2050 - - elif candidate == CAR.F_150_MK14: - # required trim only on SuperCrew - ret.wheelbase = 3.69 - ret.steerRatio = 17.0 - ret.mass = 2000 - - elif candidate == CAR.F_150_LIGHTNING_MK1: - # required trim only on SuperCrew - ret.wheelbase = 3.70 - ret.steerRatio = 16.9 - ret.mass = 2948 - - elif candidate == CAR.MUSTANG_MACH_E_MK1: - ret.wheelbase = 2.984 - ret.steerRatio = 17.0 # guess - ret.mass = 2200 - - elif candidate == CAR.FOCUS_MK4: - ret.wheelbase = 2.7 - ret.steerRatio = 15.0 - ret.mass = 1350 - - elif candidate == CAR.MAVERICK_MK1: - ret.wheelbase = 3.076 - ret.steerRatio = 17.0 - ret.mass = 1650 - - else: - raise ValueError(f"Unsupported car: {candidate}") - # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs: @@ -106,11 +63,14 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) - events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.manumatic]) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + + events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) if not self.CS.vehicle_sensors_valid: events.add(car.CarEvent.EventName.vehicleSensorsInvalid) - if self.CS.unsupported_platform: - events.add(car.CarEvent.EventName.startupNoControl) ret.events = events.to_msg() diff --git a/selfdrive/car/ford/tests/__init__.py b/selfdrive/car/ford/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/car/ford/tests/test_ford.py b/selfdrive/car/ford/tests/test_ford.py new file mode 100644 index 0000000..2ad3f5d --- /dev/null +++ b/selfdrive/car/ford/tests/test_ford.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import unittest +from parameterized import parameterized +from collections.abc import Iterable + +import capnp + +from cereal import car +from openpilot.selfdrive.car.ford.values import FW_QUERY_CONFIG +from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu + + +ECU_ADDRESSES = { + Ecu.eps: 0x730, # Power Steering Control Module (PSCM) + Ecu.abs: 0x760, # Anti-Lock Brake System (ABS) + Ecu.fwdRadar: 0x764, # Cruise Control Module (CCM) + Ecu.fwdCamera: 0x706, # Image Processing Module A (IPMA) + Ecu.engine: 0x7E0, # Powertrain Control Module (PCM) + Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM) + Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM) +} + + +ECU_FW_CORE = { + Ecu.eps: [ + b"14D003", + ], + Ecu.abs: [ + b"2D053", + ], + Ecu.fwdRadar: [ + b"14D049", + ], + Ecu.fwdCamera: [ + b"14F397", # Ford Q3 + b"14H102", # Ford Q4 + ], + Ecu.engine: [ + b"14C204", + ], +} + + +class TestFordFW(unittest.TestCase): + def test_fw_query_config(self): + for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus: + self.assertIn(ecu, ECU_ADDRESSES, "Unknown ECU") + self.assertEqual(addr, ECU_ADDRESSES[ecu], "ECU address mismatch") + self.assertIsNone(subaddr, "Unexpected ECU subaddress") + + @parameterized.expand(FW_VERSIONS.items()) + def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[capnp.lib.capnp._EnumModule, int, int | None], Iterable[bytes]]): + for (ecu, addr, subaddr), fws in fw_versions.items(): + self.assertIn(ecu, ECU_FW_CORE, "Unexpected ECU") + self.assertEqual(addr, ECU_ADDRESSES[ecu], "ECU address mismatch") + self.assertIsNone(subaddr, "Unexpected ECU subaddress") + + # Software part number takes the form: PREFIX-CORE-SUFFIX + # Prefix changes based on the family of part. It includes the model year + # and likely the platform. + # Core identifies the type of the item (e.g. 14D003 = PSCM, 14C204 = PCM). + # Suffix specifies the version of the part. -AA would be followed by -AB. + # Small increments in the suffix are usually compatible. + # Details: https://forscan.org/forum/viewtopic.php?p=70008#p70008 + for fw in fws: + self.assertEqual(len(fw), 24, "Expected ECU response to be 24 bytes") + + # TODO: parse with regex, don't need detailed error message + fw_parts = fw.rstrip(b'\x00').split(b'-') + self.assertEqual(len(fw_parts), 3, "Expected FW to be in format: prefix-core-suffix") + + prefix, core, suffix = fw_parts + self.assertEqual(len(prefix), 4, "Expected FW prefix to be 4 characters") + self.assertIn(len(core), (5, 6), "Expected FW core to be 5-6 characters") + self.assertIn(core, ECU_FW_CORE[ecu], f"Unexpected FW core for {ecu}") + self.assertIn(len(suffix), (2, 3), "Expected FW suffix to be 2-3 characters") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index b3ba607..cdf9b21 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,13 +1,13 @@ -from collections import defaultdict -from dataclasses import dataclass -from enum import Enum, StrEnum -from typing import Dict, List, Union +import copy +from dataclasses import dataclass, field, replace +from enum import Enum, IntFlag +import panda.python.uds as uds from cereal import car -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \ +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, dbc_dict, DbcDict, PlatformConfig, Platforms +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \ Device -from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries +from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -41,18 +41,9 @@ class CarControllerParams: pass -class CAR(StrEnum): - BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN" - ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" - EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" - F_150_MK14 = "FORD F-150 14TH GEN" - FOCUS_MK4 = "FORD FOCUS 4TH GEN" - MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" - F_150_LIGHTNING_MK1 = "FORD F-150 LIGHTNING 1ST GEN" - MUSTANG_MACH_E_MK1 = "FORD MUSTANG MACH-E 1ST GEN" - - -CANFD_CAR = {CAR.F_150_MK14, CAR.F_150_LIGHTNING_MK1, CAR.MUSTANG_MACH_E_MK1} +class FordFlags(IntFlag): + # Static flags + CANFD = 1 class RADAR: @@ -60,14 +51,6 @@ class RADAR: DELPHI_MRR = 'FORD_CADS' -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR)) - -# F-150 radar is not yet supported -DBC[CAR.F_150_MK14] = dbc_dict("ford_lincoln_base_pt", None) -DBC[CAR.F_150_LIGHTNING_MK1] = dbc_dict("ford_lincoln_base_pt", None) -DBC[CAR.MUSTANG_MACH_E_MK1] = dbc_dict("ford_lincoln_base_pt", None) - - class Footnote(Enum): FOCUS = CarFootnote( "Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " + @@ -77,36 +60,120 @@ class Footnote(Enum): @dataclass -class FordCarInfo(CarInfo): +class FordCarDocs(CarDocs): package: str = "Co-Pilot360 Assist+" + hybrid: bool = False + plug_in_hybrid: bool = False def init_make(self, CP: car.CarParams): - harness = CarHarness.ford_q4 if CP.carFingerprint in CANFD_CAR else CarHarness.ford_q3 - if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14): + harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3 + if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14, CAR.F_150_LIGHTNING_MK1): self.car_parts = CarParts([Device.threex_angled_mount, harness]) else: self.car_parts = CarParts([Device.threex, harness]) -CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { - CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"), - CAR.ESCAPE_MK4: [ - FordCarInfo("Ford Escape 2020-22"), - FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"), - ], - CAR.EXPLORER_MK6: [ - FordCarInfo("Ford Explorer 2020-23"), - FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"), - ], - CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"), - CAR.F_150_LIGHTNING_MK1: FordCarInfo("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0"), - CAR.MUSTANG_MACH_E_MK1: FordCarInfo("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0"), - CAR.FOCUS_MK4: FordCarInfo("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS]), - CAR.MAVERICK_MK1: [ - FordCarInfo("Ford Maverick 2022", "LARIAT Luxury"), - FordCarInfo("Ford Maverick 2023", "Co-Pilot360 Assist"), - ], -} +@dataclass +class FordPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR)) + + def init(self): + for car_docs in list(self.car_docs): + if car_docs.hybrid: + name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}" + self.car_docs.append(replace(copy.deepcopy(car_docs), name=name)) + if car_docs.plug_in_hybrid: + name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}" + self.car_docs.append(replace(copy.deepcopy(car_docs), name=name)) + + +@dataclass +class FordCANFDPlatformConfig(FordPlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None)) + + def init(self): + super().init() + self.flags |= FordFlags.CANFD + + +class CAR(Platforms): + BRONCO_SPORT_MK1 = FordPlatformConfig( + "FORD BRONCO SPORT 1ST GEN", + [FordCarDocs("Ford Bronco Sport 2021-23")], + CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7), + ) + ESCAPE_MK4 = FordPlatformConfig( + "FORD ESCAPE 4TH GEN", + [ + FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True), + FordCarDocs("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True), + ], + CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7), + ) + EXPLORER_MK6 = FordPlatformConfig( + "FORD EXPLORER 6TH GEN", + [ + FordCarDocs("Ford Explorer 2020-23", hybrid=True), # Hybrid: Limited and Platinum only + FordCarDocs("Lincoln Aviator 2020-23", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only + ], + CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8), + ) + F_150_MK14 = FordCANFDPlatformConfig( + "FORD F-150 14TH GEN", + [FordCarDocs("Ford F-150 2022-23", "Co-Pilot360 Active 2.0", hybrid=True)], + CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0), + ) + F_150_LIGHTNING_MK1 = FordCANFDPlatformConfig( + "FORD F-150 LIGHTNING 1ST GEN", + [FordCarDocs("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0")], + CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9), + ) + FOCUS_MK4 = FordPlatformConfig( + "FORD FOCUS 4TH GEN", + [FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only + CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0), + ) + MAVERICK_MK1 = FordPlatformConfig( + "FORD MAVERICK 1ST GEN", + [ + FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True), + FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True), + ], + CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0), + ) + MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig( + "FORD MUSTANG MACH-E 1ST GEN", + [FordCarDocs("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0")], + CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio + ) + + +DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00 + +ASBUILT_BLOCKS: list[tuple[int, list]] = [ + (1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]), + (2, [Ecu.abs, Ecu.debug, Ecu.eps]), + (3, [Ecu.abs, Ecu.debug, Ecu.eps]), + (4, [Ecu.debug, Ecu.fwdCamera]), + (5, [Ecu.debug]), + (6, [Ecu.debug]), + (7, [Ecu.debug]), + (8, [Ecu.debug]), + (9, [Ecu.debug]), + (16, [Ecu.debug, Ecu.fwdCamera]), + (18, [Ecu.fwdCamera]), + (20, [Ecu.fwdCamera]), + (21, [Ecu.fwdCamera]), +] + + +def ford_asbuilt_block_request(block_id: int): + return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1) + + +def ford_asbuilt_block_response(block_id: int): + return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -115,13 +182,30 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], + logging=True, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], bus=0, auxiliary=True, ), + *[Request( + [StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)], + [StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)], + whitelist_ecus=ecus, + bus=0, + logging=True, + ) for block_id, ecus in ASBUILT_BLOCKS], ], extra_ecus=[ - # We are unlikely to get a response from the PCM from behind the gateway - (Ecu.engine, 0x7e0, None), - (Ecu.shiftByWire, 0x732, None), + (Ecu.engine, 0x7e0, None), # Powertrain Control Module + # Note: We are unlikely to get a response from behind the gateway + (Ecu.shiftByWire, 0x732, None), # Gear Shift Module + (Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module ], ) + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py index 36e6794..236ade4 100755 --- a/selfdrive/car/fw_query_definitions.py +++ b/selfdrive/car/fw_query_definitions.py @@ -3,16 +3,16 @@ import capnp import copy from dataclasses import dataclass, field import struct -from typing import Callable, Dict, List, Optional, Set, Tuple +from collections.abc import Callable import panda.python.uds as uds -AddrType = Tuple[int, Optional[int]] -EcuAddrBusType = Tuple[int, Optional[int], int] -EcuAddrSubAddr = Tuple[int, int, Optional[int]] +AddrType = tuple[int, int | None] +EcuAddrBusType = tuple[int, int | None, int] +EcuAddrSubAddr = tuple[int, int, int | None] -LiveFwVersions = Dict[AddrType, Set[bytes]] -OfflineFwVersions = Dict[str, Dict[EcuAddrSubAddr, List[bytes]]] +LiveFwVersions = dict[AddrType, set[bytes]] +OfflineFwVersions = dict[str, dict[EcuAddrSubAddr, list[bytes]]] # A global list of addresses we will only ever consider for VIN responses # engine, hybrid controller, Ford abs, Hyundai CAN FD cluster, 29-bit engine, PGM-FI @@ -47,6 +47,11 @@ class StdQueries: MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + SUPPLIER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ @@ -71,9 +76,9 @@ class StdQueries: @dataclass class Request: - request: List[bytes] - response: List[bytes] - whitelist_ecus: List[int] = field(default_factory=list) + request: list[bytes] + response: list[bytes] + whitelist_ecus: list[int] = field(default_factory=list) rx_offset: int = 0x8 bus: int = 1 # Whether this query should be run on the first auxiliary panda (CAN FD cars for example) @@ -86,15 +91,15 @@ class Request: @dataclass class FwQueryConfig: - requests: List[Request] + requests: list[Request] # TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus # Overrides and removes from essential ecus for specific models and ecus (exact matching) - non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict) + non_essential_ecus: dict[capnp.lib.capnp._EnumModule, list[str]] = field(default_factory=dict) # Ecus added for data collection, not to be fingerprinted on - extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list) + extra_ecus: list[tuple[capnp.lib.capnp._EnumModule, int, int | None]] = field(default_factory=list) # Function a brand can implement to provide better fuzzy matching. Takes in FW versions, # returns set of candidates. Only will match if one candidate is returned - match_fw_to_car_fuzzy: Optional[Callable[[LiveFwVersions, OfflineFwVersions], Set[str]]] = None + match_fw_to_car_fuzzy: Callable[[LiveFwVersions, OfflineFwVersions], set[str]] | None = None def __post_init__(self): for i in range(len(self.requests)): diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 6c02e69..c200528 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,18 +1,20 @@ #!/usr/bin/env python3 from collections import defaultdict -from typing import Any, DefaultDict, Dict, Iterator, List, Optional, Set, TypeVar +from collections.abc import Iterator +from typing import Any, Protocol, TypeVar + from tqdm import tqdm import capnp import panda.python.uds as uds from cereal import car from openpilot.common.params import Params -from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs -from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig -from openpilot.selfdrive.car.interfaces import get_interface_attr -from openpilot.selfdrive.car.fingerprints import FW_VERSIONS -from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs +from openpilot.selfdrive.car.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions +from openpilot.selfdrive.car.interfaces import get_interface_attr +from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery Ecu = car.CarParams.Ecu ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] @@ -27,19 +29,18 @@ REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for T = TypeVar('T') -def chunks(l: List[T], n: int = 128) -> Iterator[List[T]]: +def chunks(l: list[T], n: int = 128) -> Iterator[list[T]]: for i in range(0, len(l), n): yield l[i:i + n] -def is_brand(brand: str, filter_brand: Optional[str]) -> bool: +def is_brand(brand: str, filter_brand: str | None) -> bool: """Returns if brand matches filter_brand or no brand filter is specified""" return filter_brand is None or brand == filter_brand -def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder], - filter_brand: Optional[str] = None) -> Dict[AddrType, Set[bytes]]: - fw_versions_dict: DefaultDict[AddrType, Set[bytes]] = defaultdict(set) +def build_fw_dict(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], filter_brand: str = None) -> dict[AddrType, set[bytes]]: + fw_versions_dict: defaultdict[AddrType, set[bytes]] = defaultdict(set) for fw in fw_versions: if is_brand(fw.brand, filter_brand) and not fw.logging: sub_addr = fw.subAddress if fw.subAddress != 0 else None @@ -47,7 +48,12 @@ def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder], return dict(fw_versions_dict) -def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude=None): +class MatchFwToCar(Protocol): + def __call__(self, live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True) -> set[str]: + ... + + +def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True, exclude: str = None) -> set[str]: """Do a fuzzy FW match. This function will return a match, and the number of firmware version that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars the match is rejected.""" @@ -72,7 +78,7 @@ def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude= all_fw_versions[(addr[1], addr[2], f)].append(candidate) matched_ecus = set() - candidate = None + match: str | None = None for addr, versions in live_fw_versions.items(): ecu_key = (addr[0], addr[1]) for version in versions: @@ -81,23 +87,23 @@ def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude= if len(candidates) == 1: matched_ecus.add(ecu_key) - if candidate is None: - candidate = candidates[0] + if match is None: + match = candidates[0] # We uniquely matched two different cars. No fuzzy match possible - elif candidate != candidates[0]: + elif match != candidates[0]: return set() # Note that it is possible to match to a candidate without all its ECUs being present # if there are enough matches. FIXME: parameterize this or require all ECUs to exist like exact matching - if len(matched_ecus) >= 2: + if match and len(matched_ecus) >= 2: if log: - cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {len(matched_ecus)} matching ECUs") - return {candidate} + cloudlog.error(f"Fingerprinted {match} using fuzzy match. {len(matched_ecus)} matching ECUs") + return {match} else: return set() -def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True, extra_fw_versions=None) -> Set[str]: +def match_fw_to_car_exact(live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True, extra_fw_versions: dict = None) -> set[str]: """Do an exact FW match. Returns all cars that match the given FW versions for a list of "essential" ECUs. If an ECU is not considered essential the FW version can be missing to get a fingerprint, but if it's present it @@ -138,9 +144,10 @@ def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True, extra_fw return set(candidates.keys()) - invalid -def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): +def match_fw_to_car(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], allow_exact: bool = True, allow_fuzzy: bool = True, + log: bool = True) -> tuple[bool, set[str]]: # Try exact matching first - exact_matches = [] + exact_matches: list[tuple[bool, MatchFwToCar]] = [] if allow_exact: exact_matches = [(True, match_fw_to_car_exact)] if allow_fuzzy: @@ -148,7 +155,7 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): for exact_match, match_func in exact_matches: # For each brand, attempt to fingerprint using all FW returned from its queries - matches = set() + matches: set[str] = set() for brand in VERSIONS.keys(): fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand) matches |= match_func(fw_versions_dict, match_brand=brand, log=log) @@ -164,12 +171,12 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): return True, set() -def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]: +def get_present_ecus(logcan, sendcan, num_pandas: int = 1) -> set[EcuAddrBusType]: params = Params() # queries are split by OBD multiplexing mode - queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []} - parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []} - responses = set() + queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []} + parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []} + responses: set[EcuAddrBusType] = set() for brand, config, r in REQUESTS: # Skip query if no panda available @@ -203,7 +210,7 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]: return ecu_responses -def get_brand_ecu_matches(ecu_rx_addrs: Set[EcuAddrBusType]) -> dict[str, set[AddrType]]: +def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, set[AddrType]]: """Returns dictionary of brands and matches with ECUs in their FW versions""" brand_addrs = {brand: {(addr, subaddr) for _, addr, subaddr in config.get_all_ecus(VERSIONS[brand])} for @@ -230,8 +237,8 @@ def set_obd_multiplexing(params: Params, obd_multiplexing: bool): cloudlog.warning("OBD multiplexing set successfully") -def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False) -> \ - List[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, + debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] @@ -253,8 +260,8 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand return all_car_fw -def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False) -> \ - List[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions(logcan, sendcan, query_brand: str = None, extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, + debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: versions = VERSIONS.copy() params = Params() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 9ebb21a..6bd703a 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -6,7 +6,8 @@ from openpilot.common.realtime import DT_CTRL 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, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, EV_CAR, SDGM_CAR +from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_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 @@ -26,7 +27,7 @@ PITCH_DEADZONE = 0.01 # [radians] 0.01 ≈ 1% grade 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 -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.start_time = 0. @@ -136,7 +137,6 @@ class CarController: self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE)) else: - # Normal operation brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V) if self.CP.carFingerprint in EV_CAR and frogpilot_variables.use_ev_tables: self.params.update_ev_gas_brake_threshold(CS.out.vEgo) @@ -188,12 +188,12 @@ class CarController: # 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, CC.enabled, 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 can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, - hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw, CS.display_menu, CS.personality_profile)) + hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw)) else: # to keep accel steady for logs when not sending gas accel += self.accel_g diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7c80476..5a5c986 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import mean from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR +from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CAMERA_ACC_CAR, CC_ONLY_CAR, SDGM_CAR TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation @@ -27,23 +27,24 @@ class CarState(CarStateBase): self.cam_lka_steering_cmd_counter = 0 self.buttons_counter = 0 + self.prev_distance_button = 0 + self.distance_button = 0 + # FrogPilot variables self.single_pedal_mode = False - # FrogPilot variables - self.display_menu = False - - self.display_timer = 0 - def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables): ret = car.CarState.new_message() self.prev_cruise_buttons = self.cruise_buttons + self.prev_distance_button = self.distance_button if self.CP.carFingerprint not in SDGM_CAR: self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] + self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] else: self.cruise_buttons = cam_cp.vl["ASCMSteeringButton"]["ACCButtons"] + self.distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"] self.buttons_counter = cam_cp.vl["ASCMSteeringButton"]["RollingCounter"] self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) # This is to avoid a fault where you engage while still moving backwards after shifting to D. @@ -167,58 +168,11 @@ class CarState(CarStateBase): ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 - # Driving personalities function - Credit goes to Mangomoose! - if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: - # Sync with the onroad UI button - if self.fpf.personality_changed_via_ui: - self.personality_profile = self.fpf.current_personality - self.previous_personality_profile = self.personality_profile - self.fpf.reset_personality_changed_param() - - # Check if the car has a camera - has_camera = self.CP.networkLocation == NetworkLocation.fwdCamera - has_camera &= not self.CP.flags & GMFlags.NO_CAMERA.value - has_camera &= not self.CP.carFingerprint in (CC_ONLY_CAR) - - if has_camera: - # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2" - self.personality_profile = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCGapLevel"] - 1 - else: - if self.CP.carFingerprint in SDGM_CAR: - distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"] - else: - distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] - - if distance_button and not self.distance_previously_pressed: - if self.display_menu: - self.personality_profile = (self.previous_personality_profile + 2) % 3 - self.display_timer = 350 - self.distance_previously_pressed = distance_button - - # Check if the display is open - if self.display_timer > 0: - self.display_timer -= 1 - self.display_menu = True - else: - self.display_menu = False - - if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0: - self.fpf.distance_button_function(self.personality_profile) - self.previous_personality_profile = self.personality_profile - - # Toggle Experimental Mode from steering wheel function - if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available: - if self.CP.carFingerprint in SDGM_CAR: - lkas_pressed = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] - else: - lkas_pressed = pt_cp.vl["ASCMSteeringButton"]["LKAButton"] - - if lkas_pressed and not self.lkas_previously_pressed: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_lkas() - else: - self.fpf.update_experimental_mode() - self.lkas_previously_pressed = lkas_pressed + self.lkas_previously_enabled = self.lkas_enabled + if self.CP.carFingerprint in SDGM_CAR: + self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] + else: + self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"] return ret @@ -285,6 +239,7 @@ class CarState(CarStateBase): messages += [ ("ASCMLKASteeringCmd", 0), ] + if CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value: messages.remove(("ECMAcceleratorPos", 80)) messages.append(("EBCMBrakePedalPosition", 100)) diff --git a/selfdrive/car/gm/fingerprints.py b/selfdrive/car/gm/fingerprints.py index e27957f..12f3061 100644 --- a/selfdrive/car/gm/fingerprints.py +++ b/selfdrive/car/gm/fingerprints.py @@ -176,6 +176,11 @@ FINGERPRINTS = { { 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8 }], + CAR.BABYENCLAVE: [ + # Buick Baby Enclave w/ ACC 2020-23 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 394: 7, 398: 8, 401: 8, 405: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 450: 4, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 456: 8, 457: 6, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 872: 1, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1201: 3, 1217: 8, 1218: 3, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1514: 8, 1517: 8, 1601: 8, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1927: 7, 1930: 7, 2018: 8, 2020: 8, 2021: 8, 2028: 8 + }], CAR.TRAX: [ { 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1930: 7 diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 4c56337..ff7538a 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -1,5 +1,6 @@ import math +from cereal import log from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import make_can_msg @@ -65,6 +66,7 @@ def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop): "GasRegenFullStopActive": at_full_stop, "GasRegenAlwaysOne": 1, "GasRegenAlwaysOne2": 1, + "GasRegenAlwaysOne3": 1, } dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2] @@ -105,18 +107,17 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) -def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw, display, personality_profile): +def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw): target_speed = min(target_speed_kph, 255) values = { "ACCAlwaysOne": 1, "ACCResumeButton": 0, - "DisplayDistance": display, "ACCSpeedSetpoint": target_speed, - "ACCGapLevel": min(personality_profile + 1, 3), # 3 "far", 0 "inactive" + "ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive" "ACCCmdActive": enabled, "ACCAlwaysOne2": 1, - "ACCLeadCar": lead_car_in_sight, + "ACCLeadCar": hud_control.leadVisible, "FCWAlert": 0x3 if fcw else 0 } @@ -178,31 +179,36 @@ def create_lka_icon_command(bus, active, critical, steer): def create_gm_cc_spam_command(packer, controller, CS, actuators): - # TODO: Cleanup the timing - normal is every 30ms... + if controller.params_.get_bool("IsMetric"): + _CV = CV.MS_TO_KPH + RATE_UP_MAX = 0.04 + RATE_DOWN_MAX = 0.04 + else: + _CV = CV.MS_TO_MPH + RATE_UP_MAX = 0.2 + RATE_DOWN_MAX = 0.2 + + accel = actuators.accel * _CV # m/s/s to mph/s + speedSetPoint = int(round(CS.out.cruiseState.speed * _CV)) cruiseBtn = CruiseButtons.INIT - - # if controller.params_.get_bool("IsMetric"): - # accel = actuators.accel * CV.MS_TO_KPH # m/s/s to km/h/s - # else: - # accel = actuators.accel * CV.MS_TO_MPH # m/s/s to mph/s - accel = actuators.accel * CV.MS_TO_MPH # m/s/s to mph/s - speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH)) - - RATE_UP_MAX = 0.2 # may be lower on new/euro cars - RATE_DOWN_MAX = 0.2 # may be lower on new/euro cars - if speedSetPoint == CS.CP.minEnableSpeed and accel < -1: cruiseBtn = CruiseButtons.CANCEL controller.apply_speed = 0 rate = 0.04 elif accel < 0: cruiseBtn = CruiseButtons.DECEL_SET - rate = max(-1 / accel, RATE_DOWN_MAX) + if speedSetPoint > (CS.out.vEgo * _CV) + 3.0: # If accel is changing directions, bring set speed to current speed as fast as possible + rate = RATE_DOWN_MAX + else: + rate = max(-1 / accel, RATE_DOWN_MAX) controller.apply_speed = speedSetPoint - 1 elif accel > 0: cruiseBtn = CruiseButtons.RES_ACCEL - rate = max(1 / accel, RATE_UP_MAX) + if speedSetPoint < (CS.out.vEgo * _CV) - 3.0: + rate = RATE_UP_MAX + else: + rate = max(1 / accel, RATE_UP_MAX) controller.apply_speed = speedSetPoint + 1 else: controller.apply_speed = speedSetPoint @@ -210,7 +216,6 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators): # Check rlogs closely - our message shouldn't show up on the pt bus for us # Or bus 2, since we're forwarding... but I think it does - # TODO: Cleanup the timing - normal is every 30ms... if (cruiseBtn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate): controller.last_button_frame = controller.frame idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index fcce13e..dc277b0 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import os -from cereal import car +from cereal import car, custom from math import fabs, exp from panda import Panda @@ -19,12 +19,12 @@ TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type - -ACCELERATOR_POS_MSG = 0xbe +PEDAL_MSG = 0x201 CAM_MSG = 0x320 # AEBCmd # TODO: Is this always linked to camera presence? -PEDAL_MSG = 0x201 +ACCELERATOR_POS_MSG = 0xbe NON_LINEAR_TORQUE_PARAMS = { CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], @@ -83,8 +83,8 @@ class CarInterface(CarInterfaceBase): return float(self.neural_ff_model.predict(inputs)) + friction def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: - if self.CP.carFingerprint in [CAR.BOLT_EUV, CAR.BOLT_CC]: - self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, CAR.BOLT_EUV) + if self.CP.carFingerprint in (CAR.BOLT_EUV, CAR.BOLT_CC): + self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint) return self.torque_from_lateral_accel_neural elif self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: return self.torque_from_lateral_accel_siglin @@ -92,18 +92,17 @@ class CarInterface(CarInterfaceBase): return self.torque_from_lateral_accel_linear @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): + params.put_bool("HideDisableOpenpilotLongitudinal", candidate not in (SDGM_CAR | CAMERA_ACC_CAR)) + ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] - if PEDAL_MSG in fingerprint[0]: ret.enableGasInterceptor = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR - useEVTables = params.get_bool("EVTable") - if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct else: @@ -137,7 +136,7 @@ class CarInterface(CarInterfaceBase): if experimental_long: ret.pcmCruise = False - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG elif candidate in SDGM_CAR: @@ -150,17 +149,18 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM else: # ASCM, OBD-II harness - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS - ret.minSteerSpeed = (6.7 if useEVTables else 7) * CV.MPH_TO_MS + ret.minSteerSpeed = 7 * CV.MPH_TO_MS # Tuning ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiV = [0.36] + if ret.enableGasInterceptor: # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_ASCM_LONG @@ -170,20 +170,13 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet - ret.tireStiffnessFactor = 0.444 # not optimized yet ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking if candidate in (CAR.VOLT, CAR.VOLT_CC): - ret.minEnableSpeed = -1 if params.get_bool("LowerVolt") else ret.minEnableSpeed - ret.mass = 1607. - ret.wheelbase = 2.69 - ret.steerRatio = 17.7 # Stock 15.7, LiveParameters - ret.tireStiffnessFactor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters - ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh - + ret.minEnableSpeed = -1 ret.lateralTuning.pid.kpBP = [0., 40.] ret.lateralTuning.pid.kpV = [0., 0.17] ret.lateralTuning.pid.kiBP = [0.] @@ -191,74 +184,20 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() ret.steerActuatorDelay = 0.2 - # softer long tune for ev table - if useEVTables: - ret.longitudinalTuning.kpBP = [5., 15., 35.] - ret.longitudinalTuning.kpV = [0.65, .9, 0.8] - ret.longitudinalTuning.kiBP = [5., 15.] - ret.longitudinalTuning.kiV = [0.04, 0.1] - ret.steerActuatorDelay = 0.18 - ret.stoppingDecelRate = 0.02 # brake_travel/s while trying to stop - ret.stopAccel = -0.5 - ret.startAccel = 0.8 - ret.vEgoStopping = 0.1 - - elif candidate == CAR.MALIBU: - ret.mass = 1496. - ret.wheelbase = 2.83 - ret.steerRatio = 15.8 - ret.centerToFront = ret.wheelbase * 0.4 # wild guess - - elif candidate == CAR.HOLDEN_ASTRA: - ret.mass = 1363. - ret.wheelbase = 2.662 - # Remaining parameters copied from Volt for now - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 15.7 - elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm - ret.mass = 4353. * CV.LB_TO_KG - ret.wheelbase = 2.86 - ret.steerRatio = 14.4 # end to end is 13.46 - ret.centerToFront = ret.wheelbase * 0.4 ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.BUICK_LACROSSE: - ret.mass = 1712. - ret.wheelbase = 2.91 - ret.steerRatio = 15.8 - ret.centerToFront = ret.wheelbase * 0.4 # wild guess CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.BUICK_REGAL: - ret.mass = 3779. * CV.LB_TO_KG # (3849+3708)/2 - ret.wheelbase = 2.83 # 111.4 inches in meters - ret.steerRatio = 14.4 # guess for tourx - ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx - - elif candidate == CAR.CADILLAC_ATS: - ret.mass = 1601. - ret.wheelbase = 2.78 - ret.steerRatio = 15.3 - ret.centerToFront = ret.wheelbase * 0.5 - elif candidate == CAR.ESCALADE: ret.minEnableSpeed = -1. # engage speed is decided by pcm - ret.mass = 5653. * CV.LB_TO_KG # (5552+5815)/2 - ret.wheelbase = 2.95 # 116 inches in meters - ret.steerRatio = 17.3 - ret.centerToFront = ret.wheelbase * 0.5 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019): ret.minEnableSpeed = -1. # engage speed is decided by pcm - ret.mass = 2739. - ret.wheelbase = 3.302 - ret.steerRatio = 17.3 - ret.centerToFront = ret.wheelbase * 0.5 - ret.tireStiffnessFactor = 1.0 if candidate == CAR.ESCALADE_ESV: ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] @@ -269,11 +208,6 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.BOLT_EUV, CAR.BOLT_CC): - ret.mass = 1669. - ret.wheelbase = 2.63779 - ret.steerRatio = 16.8 - ret.centerToFront = ret.wheelbase * 0.4 - ret.tireStiffnessFactor = 1.0 ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) @@ -282,11 +216,6 @@ class CarInterface(CarInterfaceBase): ret.flags |= GMFlags.PEDAL_LONG.value elif candidate == CAR.SILVERADO: - ret.mass = 2450. - ret.wheelbase = 3.75 - ret.steerRatio = 16.3 - ret.centerToFront = ret.wheelbase * 0.5 - ret.tireStiffnessFactor = 1.0 # On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop # with foot on brake to allow engagement, but this platform only has that check in the camera. # TODO: check if this is split by EV/ICE with more platforms in the future @@ -295,61 +224,33 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.EQUINOX, CAR.EQUINOX_CC): - ret.mass = 3500. * CV.LB_TO_KG - ret.wheelbase = 2.72 - ret.steerRatio = 14.4 - ret.centerToFront = ret.wheelbase * 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.TRAILBLAZER, CAR.TRAILBLAZER_CC): - ret.mass = 1345. - ret.wheelbase = 2.64 - ret.steerRatio = 16.8 - ret.centerToFront = ret.wheelbase * 0.4 - ret.tireStiffnessFactor = 1.0 ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.SUBURBAN, CAR.SUBURBAN_CC): - ret.mass = 2731. - ret.wheelbase = 3.302 - ret.steerRatio = 17.3 # COPIED FROM SILVERADO - ret.centerToFront = ret.wheelbase * 0.49 ret.steerActuatorDelay = 0.075 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.YUKON_CC: - ret.minSteerSpeed = -1 * CV.MPH_TO_MS - ret.mass = 5602. * CV.LB_TO_KG # (3849+3708)/2 - ret.wheelbase = 2.95 # 116 inches in meters - ret.steerRatio = 16.3 # guess for tourx - ret.steerRatioRear = 0. # unknown online - ret.centerToFront = 2.59 # ret.wheelbase * 0.4 # wild guess ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.XT4: - ret.mass = 3660. * CV.LB_TO_KG - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 - ret.centerToFront = ret.wheelbase * 0.4 ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.BABYENCLAVE: + ret.steerActuatorDelay = 0.2 + ret.minSteerSpeed = 10 * CV.KPH_TO_MS + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.CT6_CC: - ret.wheelbase = 3.11 - ret.mass = 5198. * CV.LB_TO_KG - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 17.7 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.TRAX: - ret.mass = 1365. - ret.wheelbase = 2.7 - ret.steerRatio = 16.4 - ret.centerToFront = ret.wheelbase * 0.4 - ret.tireStiffnessFactor = 1.0 - ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) if ret.enableGasInterceptor: @@ -357,7 +258,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM ret.minEnableSpeed = -1 ret.pcmCruise = False - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = not disable_openpilot_long ret.stoppingControl = True ret.autoResumeSng = True @@ -383,7 +284,7 @@ class CarInterface(CarInterfaceBase): ret.radarUnavailable = True ret.experimentalLongitudinalAvailable = False ret.minEnableSpeed = 24 * CV.MPH_TO_MS - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = not disable_openpilot_long ret.pcmCruise = False ret.longitudinalTuning.deadzoneBP = [0.] @@ -400,6 +301,9 @@ class CarInterface(CarInterfaceBase): if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC + if ACCELERATOR_POS_MSG not in fingerprint[CanBus.POWERTRAIN]: + ret.flags |= GMFlags.NO_ACCELERATOR_POS_MSG.value + # Exception for flashed cars, or cars whose camera was removed if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and CAM_MSG not in fingerprint[CanBus.CAMERA] and not candidate in SDGM_CAR: ret.flags |= GMFlags.NO_CAMERA.value @@ -416,11 +320,16 @@ class CarInterface(CarInterfaceBase): # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: - ret.buttonEvents = create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, - unpressed_btn=CruiseButtons.UNPRESS) + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, + unpressed_btn=CruiseButtons.UNPRESS), + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, + {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # The ECM allows enabling on falling edge of set, but only rising edge of resume - events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.sport, GearShifter.low, + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.eco, GearShifter.manumatic], pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,)) if not self.CP.pcmCruise: @@ -431,9 +340,9 @@ class CarInterface(CarInterfaceBase): # TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and - (self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)): + (self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)): events.add(EventName.belowEngageSpeed) - if ret.cruiseState.standstill and not self.CP.autoResumeSng and not self.disable_resumeRequired: + if ret.cruiseState.standstill and not self.disable_resumeRequired and not self.CP.autoResumeSng: events.add(EventName.resumeRequired) self.resumeRequired_shown = True @@ -446,7 +355,7 @@ class CarInterface(CarInterfaceBase): self.belowSteerSpeed_shown = True # Disable the "belowSteerSpeed" event after it's been shown once to not annoy the driver - if self.belowSteerSpeed_shown and ret.vEgo > self.CP.minSteerSpeed: + if self.belowSteerSpeed_shown and ret.vEgo >= self.CP.minSteerSpeed: self.disable_belowSteerSpeed = True if (self.CP.flags & GMFlags.CC_LONG.value) and ret.vEgo < self.CP.minEnableSpeed and ret.cruiseState.enabled: diff --git a/selfdrive/car/gm/tests/__init__.py b/selfdrive/car/gm/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/car/gm/tests/test_gm.py b/selfdrive/car/gm/tests/test_gm.py new file mode 100644 index 0000000..2aea5b2 --- /dev/null +++ b/selfdrive/car/gm/tests/test_gm.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +from parameterized import parameterized +import unittest + +from openpilot.selfdrive.car.gm.fingerprints import FINGERPRINTS +from openpilot.selfdrive.car.gm.values import CAMERA_ACC_CAR, CAR, GM_RX_OFFSET + +CAMERA_DIAGNOSTIC_ADDRESS = 0x24b + + +class TestGMFingerprint(unittest.TestCase): + @parameterized.expand(FINGERPRINTS.items()) + def test_can_fingerprints(self, car_model, fingerprints): + self.assertGreater(len(fingerprints), 0) + + # Trailblazer is in dashcam + if car_model != CAR.TRAILBLAZER: + self.assertTrue(all(len(finger) for finger in fingerprints)) + + # The camera can sometimes be communicating on startup + if car_model in CAMERA_ACC_CAR - {CAR.TRAILBLAZER}: + for finger in fingerprints: + for required_addr in (CAMERA_DIAGNOSTIC_ADDRESS, CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET): + self.assertEqual(finger.get(required_addr), 8, required_addr) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 9af424e..65a278d 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -1,13 +1,11 @@ -from collections import defaultdict -from dataclasses import dataclass -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Union +from dataclasses import dataclass, field +from enum import Enum, IntFlag from cereal import car from openpilot.common.numpy_fast import interp from openpilot.common.params import Params -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column +from openpilot.selfdrive.car import dbc_dict, PlatformConfig, DbcDict, Platforms, CarSpecs +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -60,7 +58,7 @@ class CarControllerParams: elif CP.carFingerprint in SDGM_CAR: self.MAX_GAS = 7496 - self.MAX_GAS_PLUS = 7496 + self.MAX_GAS_PLUS = 8848 self.MAX_ACC_REGEN = 5610 self.INACTIVE_REGEN = 5650 max_regen_acceleration = 0. @@ -95,35 +93,6 @@ class CarControllerParams: self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold] -class CAR(StrEnum): - HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" - VOLT = "CHEVROLET VOLT PREMIER 2017" - CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" - MALIBU = "CHEVROLET MALIBU PREMIER 2017" - ACADIA = "GMC ACADIA DENALI 2018" - BUICK_LACROSSE = "BUICK LACROSSE 2017" - BUICK_REGAL = "BUICK REGAL ESSENCE 2018" - ESCALADE = "CADILLAC ESCALADE 2017" - ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" - ESCALADE_ESV_2019 = "CADILLAC ESCALADE ESV 2019" - BOLT_EUV = "CHEVROLET BOLT EUV 2022" - SILVERADO = "CHEVROLET SILVERADO 1500 2020" - EQUINOX = "CHEVROLET EQUINOX 2019" - TRAILBLAZER = "CHEVROLET TRAILBLAZER 2021" - # Separate car def is required when there is no ASCM - # (for now) unless there is a way to detect it when it has been unplugged... - VOLT_CC = "CHEVROLET VOLT NO ACC" - BOLT_CC = "CHEVROLET BOLT EV NO ACC" - EQUINOX_CC = "CHEVROLET EQUINOX NO ACC" - SUBURBAN = "CHEVROLET SUBURBAN PREMIER 2016" - SUBURBAN_CC = "CHEVROLET SUBURBAN NO ACC" - YUKON_CC = "GMC YUKON NO ACC" - CT6_CC = "CADILLAC CT6 NO ACC" - TRAILBLAZER_CC = "CHEVROLET TRAILBLAZER 2024 NO ACC" - XT4 = "CADILLAC XT4 2023" - TRAX = "CHEVROLET TRAX 2024" - - class Footnote(Enum): OBD_II = CarFootnote( 'Requires a community built ASCM harness. ' + @@ -132,7 +101,7 @@ class Footnote(Enum): @dataclass -class GMCarInfo(CarInfo): +class GMCarDocs(CarDocs): package: str = "Adaptive Cruise Control (ACC)" def init_make(self, CP: car.CarParams): @@ -143,39 +112,151 @@ class GMCarInfo(CarInfo): self.footnotes.append(Footnote.OBD_II) -CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { - CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"), - CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ"), - CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"), - CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"), - CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), - CAR.BUICK_LACROSSE: GMCarInfo("Buick LaCrosse 2017-19", "Driver Confidence Package 2"), - CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), - CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"), - CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), - CAR.ESCALADE_ESV_2019: GMCarInfo("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS"), - CAR.BOLT_EUV: [ - GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"), - GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), - ], - CAR.SILVERADO: [ - GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"), - GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), - ], - CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22"), - CAR.TRAILBLAZER: GMCarInfo("Chevrolet Trailblazer 2021-22"), +@dataclass(frozen=True, kw_only=True) +class GMCarSpecs(CarSpecs): + tireStiffnessFactor: float = 0.444 # not optimized yet - CAR.VOLT_CC: GMCarInfo("Chevrolet Volt No ACC"), - CAR.BOLT_CC: GMCarInfo("Chevrolet Bolt No ACC"), - CAR.EQUINOX_CC: GMCarInfo("Chevrolet Equinox No ACC"), - CAR.SUBURBAN: GMCarInfo("Chevrolet Suburban Premier 2016-2020"), - CAR.SUBURBAN_CC: GMCarInfo("Chevrolet Suburban No ACC"), - CAR.YUKON_CC: GMCarInfo("GMC Yukon No ACC"), - CAR.CT6_CC: GMCarInfo("Cadillac CT6 No ACC"), - CAR.TRAILBLAZER_CC: GMCarInfo("Chevrolet Trailblazer 2024 No ACC"), - CAR.XT4: GMCarInfo("Cadillac XT4 2023", "Driver Assist Package"), - CAR.TRAX: GMCarInfo("Chevrolet TRAX 2024"), -} + +@dataclass +class GMPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) + + +class CAR(Platforms): + HOLDEN_ASTRA = GMPlatformConfig( + "HOLDEN ASTRA RS-V BK 2017", + [GMCarDocs("Holden Astra 2017")], + GMCarSpecs(mass=1363, wheelbase=2.662, steerRatio=15.7, centerToFrontRatio=0.4), + ) + VOLT = GMPlatformConfig( + "CHEVROLET VOLT PREMIER 2017", + [GMCarDocs("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ")], + GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469, minEnableSpeed=-1), + dbc_dict=dbc_dict('gm_global_a_powertrain_volt', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis') + ) + CADILLAC_ATS = GMPlatformConfig( + "CADILLAC ATS Premium Performance 2018", + [GMCarDocs("Cadillac ATS Premium Performance 2018")], + GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3), + ) + MALIBU = GMPlatformConfig( + "CHEVROLET MALIBU PREMIER 2017", + [GMCarDocs("Chevrolet Malibu Premier 2017")], + GMCarSpecs(mass=1496, wheelbase=2.83, steerRatio=15.8, centerToFrontRatio=0.4), + ) + ACADIA = GMPlatformConfig( + "GMC ACADIA DENALI 2018", + [GMCarDocs("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo")], + GMCarSpecs(mass=1975, wheelbase=2.86, steerRatio=14.4, centerToFrontRatio=0.4), + ) + BUICK_LACROSSE = GMPlatformConfig( + "BUICK LACROSSE 2017", + [GMCarDocs("Buick LaCrosse 2017-19", "Driver Confidence Package 2")], + GMCarSpecs(mass=1712, wheelbase=2.91, steerRatio=15.8, centerToFrontRatio=0.4), + ) + BUICK_REGAL = GMPlatformConfig( + "BUICK REGAL ESSENCE 2018", + [GMCarDocs("Buick Regal Essence 2018")], + GMCarSpecs(mass=1714, wheelbase=2.83, steerRatio=14.4, centerToFrontRatio=0.4), + ) + ESCALADE = GMPlatformConfig( + "CADILLAC ESCALADE 2017", + [GMCarDocs("Cadillac Escalade 2017", "Driver Assist Package")], + GMCarSpecs(mass=2564, wheelbase=2.95, steerRatio=17.3), + ) + ESCALADE_ESV = GMPlatformConfig( + "CADILLAC ESCALADE ESV 2016", + [GMCarDocs("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS")], + GMCarSpecs(mass=2739, wheelbase=3.302, steerRatio=17.3, tireStiffnessFactor=1.0), + ) + ESCALADE_ESV_2019 = GMPlatformConfig( + "CADILLAC ESCALADE ESV 2019", + [GMCarDocs("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS")], + ESCALADE_ESV.specs, + ) + BOLT_EUV = GMPlatformConfig( + "CHEVROLET BOLT EUV 2022", + [ + GMCarDocs("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"), + GMCarDocs("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), + ], + GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) + SILVERADO = GMPlatformConfig( + "CHEVROLET SILVERADO 1500 2020", + [ + GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"), + GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), + ], + GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0), + ) + EQUINOX = GMPlatformConfig( + "CHEVROLET EQUINOX 2019", + [GMCarDocs("Chevrolet Equinox 2019-22")], + GMCarSpecs(mass=1588, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4), + ) + TRAILBLAZER = GMPlatformConfig( + "CHEVROLET TRAILBLAZER 2021", + [GMCarDocs("Chevrolet Trailblazer 2021-22")], + GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) + # Separate car def is required when there is no ASCM + # (for now) unless there is a way to detect it when it has been unplugged... + VOLT_CC = GMPlatformConfig( + "CHEVROLET VOLT NO ACC", + [GMCarDocs("Chevrolet Volt 2017-18")], + GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=1.0), + ) + BOLT_CC = GMPlatformConfig( + "CHEVROLET BOLT EV NO ACC", + [GMCarDocs("Chevrolet Bolt No ACC")], + GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) + EQUINOX_CC = GMPlatformConfig( + "CHEVROLET EQUINOX NO ACC", + [GMCarDocs("Chevrolet Equinox No ACC")], + GMCarSpecs(mass=3500, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) + SUBURBAN = GMPlatformConfig( + "CHEVROLET SUBURBAN PREMIER 2016", + [GMCarDocs("Chevrolet Suburban Premier 2016-2020")], + CarSpecs(mass=2731, wheelbase=3.302, steerRatio=17.3, centerToFrontRatio=0.49), + ) + SUBURBAN_CC = GMPlatformConfig( + "CHEVROLET SUBURBAN NO ACC", + [GMCarDocs("Chevrolet Suburban No ACC")], + GMCarSpecs(mass=2731, wheelbase=3.032, steerRatio=17.3, centerToFrontRatio=0.49, tireStiffnessFactor=1.0), + ) + YUKON_CC = GMPlatformConfig( + "GMC YUKON NO ACC", + [GMCarDocs("GMC Yukon No ACC")], + CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4), + ) + CT6_CC = GMPlatformConfig( + "CADILLAC CT6 NO ACC", + [GMCarDocs("Cadillac CT6 No ACC")], + CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4), + ) + TRAILBLAZER_CC = GMPlatformConfig( + "CHEVROLET TRAILBLAZER NO ACC", + [GMCarDocs("Chevrolet Trailblazer 2024 No ACC")], + GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) + XT4 = GMPlatformConfig( + "CADILLAC XT4 2023", + [GMCarDocs("Cadillac XT4 2023", "Driver Assist Package")], + CarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4), + ) + BABYENCLAVE = GMPlatformConfig( + "BUICK BABY ENCLAVE 2020", + [GMCarDocs("Buick Baby Enclave 2020-23")], + CarSpecs(mass=2050, wheelbase=2.86, steerRatio=16.0, centerToFrontRatio=0.5), + ) + TRAX = GMPlatformConfig( + "CHEVROLET TRAX 2024", + [GMCarDocs("Chevrolet TRAX 2024")], + CarSpecs(mass=1365, wheelbase=2.7, steerRatio=16.4, centerToFrontRatio=0.4), + ) class CruiseButtons: @@ -206,26 +287,38 @@ class GMFlags(IntFlag): NO_CAMERA = 4 NO_ACCELERATOR_POS_MSG = 8 - # In a Data Module, an identifier is a string used to recognize an object, # either by itself or together with the identifiers of parent objects. # Each returns a 4 byte hex representation of the decimal part number. `b"\x02\x8c\xf0'"` -> 42790951 +GM_BOOT_SOFTWARE_PART_NUMER_REQUEST = b'\x1a\xc0' # likely does not contain anything useful GM_SOFTWARE_MODULE_1_REQUEST = b'\x1a\xc1' GM_SOFTWARE_MODULE_2_REQUEST = b'\x1a\xc2' GM_SOFTWARE_MODULE_3_REQUEST = b'\x1a\xc3' + +# Part number of XML data file that is used to configure ECU +GM_XML_DATA_FILE_PART_NUMBER = b'\x1a\x9c' +GM_XML_CONFIG_COMPAT_ID = b'\x1a\x9b' # used to know if XML file is compatible with the ECU software/hardware + # This DID is for identifying the part number that reflects the mix of hardware, # software, and calibrations in the ECU when it first arrives at the vehicle assembly plant. # If there's an Alpha Code, it's associated with this part number and stored in the DID $DB. GM_END_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcb' +GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdb' GM_BASE_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcc' +GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdc' GM_FW_RESPONSE = b'\x5a' GM_FW_REQUESTS = [ + GM_BOOT_SOFTWARE_PART_NUMER_REQUEST, GM_SOFTWARE_MODULE_1_REQUEST, GM_SOFTWARE_MODULE_2_REQUEST, GM_SOFTWARE_MODULE_3_REQUEST, + GM_XML_DATA_FILE_PART_NUMBER, + GM_XML_CONFIG_COMPAT_ID, GM_END_MODEL_PART_NUMBER_REQUEST, + GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST, GM_BASE_MODEL_PART_NUMBER_REQUEST, + GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST, ] GM_RX_OFFSET = 0x400 @@ -243,15 +336,11 @@ FW_QUERY_CONFIG = FwQueryConfig( extra_ecus=[(Ecu.fwdCamera, 0x24b, None)], ) -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) -DBC[CAR.VOLT] = dbc_dict('gm_global_a_powertrain_volt', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis') -DBC[CAR.VOLT_CC] = DBC[CAR.VOLT] - EV_CAR = {CAR.VOLT, CAR.BOLT_EUV, CAR.VOLT_CC, CAR.BOLT_CC} CC_ONLY_CAR = {CAR.VOLT_CC, CAR.BOLT_CC, CAR.EQUINOX_CC, CAR.SUBURBAN_CC, CAR.YUKON_CC, CAR.CT6_CC, CAR.TRAILBLAZER_CC} # We're integrated at the Safety Data Gateway Module on these cars -SDGM_CAR = {CAR.XT4} +SDGM_CAR = {CAR.XT4, CAR.BABYENCLAVE} # Slow acceleration cars SLOW_ACC = {CAR.SILVERADO} @@ -261,3 +350,5 @@ CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX, CAR.TRAILBLAZER, CAR CAMERA_ACC_CAR.update({CAR.VOLT_CC, CAR.BOLT_CC, CAR.EQUINOX_CC, CAR.YUKON_CC, CAR.CT6_CC, CAR.TRAILBLAZER_CC}) STEER_THRESHOLD = 1.0 + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 84d5de5..d1ad8ce 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -7,6 +7,7 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import create_gas_interceptor_command from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -94,8 +95,8 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", - ["pcm_accel", "v_cruise", "lead_visible", "personality_profile", - "lanes_visible", "fcw", "acc_alert", "steer_required"]) + ["pcm_accel", "v_cruise", "lead_visible", + "lanes_visible", "fcw", "acc_alert", "steer_required", "lead_distance_bars"]) def rate_limit_steer(new_steer, last_steer): @@ -104,11 +105,12 @@ def rate_limit_steer(new_steer, last_steer): return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA) -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) + self.CAN = hondacan.CanBus(CP) self.frame = 0 self.braking = False @@ -167,7 +169,7 @@ class CarController: can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # Send steering command. - can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint, + can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_steer, CC.latActive, self.CP.carFingerprint, CS.CP.openpilotLongitudinalControl)) # wind brake from air resistance decel at high speed @@ -201,12 +203,12 @@ class CarController: if not self.CP.openpilotLongitudinalControl: if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message - can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint)) + can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CAN, self.CP.carFingerprint)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint)) + can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, self.CP.carFingerprint)) elif CC.cruiseControl.resume: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) + can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) else: # Send gas and brake commands. @@ -222,7 +224,7 @@ class CarController: stopping = actuators.longControlState == LongCtrlState.stopping self.stopping_counter = self.stopping_counter + 1 if stopping else 0 - can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas, + can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, self.stopping_counter, self.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) @@ -230,7 +232,7 @@ class CarController: pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pcm_override = True - can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, + can_sends.append(hondacan.create_brake_command(self.packer, self.CAN, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, self.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake @@ -251,9 +253,9 @@ class CarController: # Send dashboard UI commands. if self.frame % 10 == 0: - hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, CS.personality_profile + 1, - hud_control.lanesVisible, fcw_display, acc_alert, steer_required) - can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive)) + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, + hud_control.lanesVisible, fcw_display, acc_alert, steer_required, hud_control.leadDistanceBars) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CAN, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive)) if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: self.speed = pcm_speed diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6d76868..60837b3 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -5,7 +5,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus +from openpilot.selfdrive.car.honda.hondacan import CanBus, get_cruise_speed_conversion from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \ HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \ HondaFlags @@ -64,7 +64,7 @@ def get_can_messages(CP, gearbox_msg): messages.append(("CRUISE_PARAMS", 50)) # TODO: clean this up - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, + if CP.carFingerprint in (CAR.ACCORD, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): pass elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): @@ -131,7 +131,7 @@ class CarState(CarStateBase): # panda checks if the signal is non-zero ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5 # TODO: find a common signal across all cars - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, + if self.CP.carFingerprint in (CAR.ACCORD, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): @@ -208,6 +208,10 @@ class CarState(CarStateBase): ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200) if self.CP.carFingerprint in HONDA_BOSCH: + # The PCM always manages its own cruise control state, but doesn't publish it + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + ret.cruiseState.nonAdaptive = cp_cam.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0 + if not self.CP.openpilotLongitudinalControl: # ACC_HUD is on camera bus on radarless cars acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"] @@ -268,40 +272,17 @@ class CarState(CarStateBase): ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1 ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 - # Driving personalities function - if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: - # Sync with the onroad UI button - if self.fpf.personality_changed_via_ui: - self.personality_profile = self.fpf.current_personality - self.previous_personality_profile = self.personality_profile - self.fpf.reset_personality_changed_param() + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_setting == 3 - # Change personality upon steering wheel button press - distance_button = self.cruise_setting == 3 - - if distance_button and not self.distance_previously_pressed: - self.personality_profile = (self.previous_personality_profile + 2) % 3 - self.distance_previously_pressed = distance_button - - if self.personality_profile != self.previous_personality_profile: - self.fpf.distance_button_function(self.personality_profile) - self.previous_personality_profile = self.personality_profile - - # Toggle Experimental Mode from steering wheel function - if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available: - lkas_pressed = self.cruise_setting == 1 - if lkas_pressed and not self.lkas_previously_pressed: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_lkas() - else: - self.fpf.update_experimental_mode() - self.lkas_previously_pressed = lkas_pressed + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = self.cruise_setting == 1 return ret def get_can_parser(self, CP): messages = get_can_messages(CP, self.gearbox_msg) - return CANParser(DBC[CP.carFingerprint]["pt"], messages, get_pt_bus(CP.carFingerprint)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).pt) @staticmethod def get_cam_can_parser(CP): @@ -310,9 +291,10 @@ class CarState(CarStateBase): ] if CP.carFingerprint in HONDA_BOSCH_RADARLESS: - messages.append(("LKAS_HUD", 10)) - if not CP.openpilotLongitudinalControl: - messages.append(("ACC_HUD", 10)) + messages += [ + ("ACC_HUD", 10), + ("LKAS_HUD", 10), + ] elif CP.carFingerprint not in HONDA_BOSCH: messages += [ @@ -321,7 +303,7 @@ class CarState(CarStateBase): ("BRAKE_COMMAND", 50), ] - return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).camera) @staticmethod def get_body_can_parser(CP): @@ -330,6 +312,6 @@ class CarState(CarStateBase): ("BSM_STATUS_LEFT", 3), ("BSM_STATUS_RIGHT", 3), ] - bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port) + bus_body = CanBus(CP).radar # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port) return CANParser(DBC[CP.carFingerprint]["body"], messages, bus_body) return None diff --git a/selfdrive/car/honda/fingerprints.py b/selfdrive/car/honda/fingerprints.py index 0d4ebd7..29fbdf1 100644 --- a/selfdrive/car/honda/fingerprints.py +++ b/selfdrive/car/honda/fingerprints.py @@ -39,6 +39,7 @@ FW_VERSIONS = { b'37805-6B2-A810\x00\x00', b'37805-6B2-A820\x00\x00', b'37805-6B2-A920\x00\x00', + b'37805-6B2-A960\x00\x00', b'37805-6B2-AA10\x00\x00', b'37805-6B2-C520\x00\x00', b'37805-6B2-C540\x00\x00', @@ -48,6 +49,7 @@ FW_VERSIONS = { ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TVC-A910\x00\x00', + b'54008-TWA-A910\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6A7-A220\x00\x00', @@ -89,6 +91,12 @@ FW_VERSIONS = { b'57114-TVA-C530\x00\x00', b'57114-TVA-E520\x00\x00', b'57114-TVE-H250\x00\x00', + b'57114-TWA-A040\x00\x00', + b'57114-TWA-A050\x00\x00', + b'57114-TWA-A530\x00\x00', + b'57114-TWA-B520\x00\x00', + b'57114-TWA-C510\x00\x00', + b'57114-TWB-H030\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBX-H120\x00\x00', @@ -100,6 +108,7 @@ FW_VERSIONS = { b'39990-TVA-X030\x00\x00', b'39990-TVA-X040\x00\x00', b'39990-TVE-H130\x00\x00', + b'39990-TWB-H120\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TBX-H230\x00\x00', @@ -108,6 +117,9 @@ FW_VERSIONS = { b'77959-TVA-H230\x00\x00', b'77959-TVA-L420\x00\x00', b'77959-TVA-X330\x00\x00', + b'77959-TWA-A440\x00\x00', + b'77959-TWA-L420\x00\x00', + b'77959-TWB-H220\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBX-H310\x00\x00', @@ -141,7 +153,19 @@ FW_VERSIONS = { b'78109-TVC-M510\x00\x00', b'78109-TVC-YF10\x00\x00', b'78109-TVE-H610\x00\x00', + b'78109-TWA-A010\x00\x00', + b'78109-TWA-A020\x00\x00', + b'78109-TWA-A030\x00\x00', + b'78109-TWA-A110\x00\x00', + b'78109-TWA-A120\x00\x00', + b'78109-TWA-A130\x00\x00', b'78109-TWA-A210\x00\x00', + b'78109-TWA-A220\x00\x00', + b'78109-TWA-A230\x00\x00', + b'78109-TWA-A610\x00\x00', + b'78109-TWA-H210\x00\x00', + b'78109-TWA-L010\x00\x00', + b'78109-TWA-L210\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ b'78209-TVA-A010\x00\x00', @@ -158,6 +182,9 @@ FW_VERSIONS = { b'36802-TVE-H070\x00\x00', b'36802-TWA-A070\x00\x00', b'36802-TWA-A080\x00\x00', + b'36802-TWA-A210\x00\x00', + b'36802-TWA-A330\x00\x00', + b'36802-TWB-H060\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TBX-H130\x00\x00', @@ -166,72 +193,17 @@ FW_VERSIONS = { b'36161-TVC-A330\x00\x00', b'36161-TVE-H050\x00\x00', b'36161-TWA-A070\x00\x00', + b'36161-TWA-A330\x00\x00', + b'36161-TWB-H040\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TVA-A010\x00\x00', b'38897-TVA-A020\x00\x00', b'38897-TVA-A230\x00\x00', b'38897-TVA-A240\x00\x00', - ], - }, - CAR.ACCORDH: { - (Ecu.gateway, 0x18daeff1, None): [ b'38897-TWA-A120\x00\x00', b'38897-TWD-J020\x00\x00', ], - (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TWA-A040\x00\x00', - b'57114-TWA-A050\x00\x00', - b'57114-TWA-A530\x00\x00', - b'57114-TWA-B520\x00\x00', - b'57114-TWA-C510\x00\x00', - b'57114-TWB-H030\x00\x00', - ], - (Ecu.srs, 0x18da53f1, None): [ - b'77959-TWA-A440\x00\x00', - b'77959-TWA-L420\x00\x00', - b'77959-TWB-H220\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TWA-A010\x00\x00', - b'78109-TWA-A020\x00\x00', - b'78109-TWA-A030\x00\x00', - b'78109-TWA-A110\x00\x00', - b'78109-TWA-A120\x00\x00', - b'78109-TWA-A130\x00\x00', - b'78109-TWA-A210\x00\x00', - b'78109-TWA-A220\x00\x00', - b'78109-TWA-A230\x00\x00', - b'78109-TWA-A610\x00\x00', - b'78109-TWA-H210\x00\x00', - b'78109-TWA-L010\x00\x00', - b'78109-TWA-L210\x00\x00', - ], - (Ecu.shiftByWire, 0x18da0bf1, None): [ - b'54008-TWA-A910\x00\x00', - ], - (Ecu.hud, 0x18da61f1, None): [ - b'78209-TVA-A010\x00\x00', - b'78209-TVA-A110\x00\x00', - ], - (Ecu.fwdCamera, 0x18dab5f1, None): [ - b'36161-TWA-A070\x00\x00', - b'36161-TWA-A330\x00\x00', - b'36161-TWB-H040\x00\x00', - ], - (Ecu.fwdRadar, 0x18dab0f1, None): [ - b'36802-TWA-A070\x00\x00', - b'36802-TWA-A080\x00\x00', - b'36802-TWA-A210\x00\x00', - b'36802-TWA-A330\x00\x00', - b'36802-TWB-H060\x00\x00', - ], - (Ecu.eps, 0x18da30f1, None): [ - b'39990-TVA-A150\x00\x00', - b'39990-TVA-A160\x00\x00', - b'39990-TVA-A340\x00\x00', - b'39990-TWB-H120\x00\x00', - ], }, CAR.CIVIC: { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ @@ -844,6 +816,7 @@ FW_VERSIONS = { b'37805-5MR-3250\x00\x00', b'37805-5MR-4070\x00\x00', b'37805-5MR-4080\x00\x00', + b'37805-5MR-4170\x00\x00', b'37805-5MR-4180\x00\x00', b'37805-5MR-A240\x00\x00', b'37805-5MR-A250\x00\x00', @@ -959,6 +932,7 @@ FW_VERSIONS = { b'54008-TG7-A530\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5EY-A040\x00\x00', b'28101-5EY-A050\x00\x00', b'28101-5EY-A100\x00\x00', b'28101-5EY-A430\x00\x00', @@ -979,6 +953,7 @@ FW_VERSIONS = { b'37805-RLV-4070\x00\x00', b'37805-RLV-5140\x00\x00', b'37805-RLV-5230\x00\x00', + b'37805-RLV-A630\x00\x00', b'37805-RLV-A830\x00\x00', b'37805-RLV-A840\x00\x00', b'37805-RLV-B210\x00\x00', @@ -1094,6 +1069,7 @@ FW_VERSIONS = { b'57114-TG7-A630\x00\x00', b'57114-TG7-A730\x00\x00', b'57114-TG8-A140\x00\x00', + b'57114-TG8-A230\x00\x00', b'57114-TG8-A240\x00\x00', b'57114-TG8-A630\x00\x00', b'57114-TG8-A730\x00\x00', diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index a7728cf..2374203 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -1,4 +1,5 @@ from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import CanBusBase from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams # CAN bus layout with relay @@ -8,15 +9,34 @@ from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_ # 3 = F-CAN A - OBDII port -def get_pt_bus(car_fingerprint): - return 1 if car_fingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) else 0 +class CanBus(CanBusBase): + def __init__(self, CP=None, fingerprint=None) -> None: + # use fingerprint if specified + super().__init__(CP if fingerprint is None else None, fingerprint) + + if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS): + self._pt, self._radar = self.offset + 1, self.offset + else: + self._pt, self._radar = self.offset, self.offset + 1 + + @property + def pt(self) -> int: + return self._pt + + @property + def radar(self) -> int: + return self._radar + + @property + def camera(self) -> int: + return self.offset + 2 -def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): +def get_lkas_cmd_bus(CAN, car_fingerprint, radar_disabled=False): no_radar = car_fingerprint in HONDA_BOSCH_RADARLESS if radar_disabled or no_radar: # when radar is disabled, steering commands are sent directly to powertrain bus - return get_pt_bus(car_fingerprint) + return CAN.pt # normally steering commands are sent to radar, which forwards them to powertrain bus return 0 @@ -26,7 +46,7 @@ def get_cruise_speed_conversion(car_fingerprint: str, is_metric: bool) -> float: return CV.MPH_TO_MS if car_fingerprint in HONDA_BOSCH_RADARLESS and not is_metric else CV.KPH_TO_MS -def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake): +def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 brake_rq = apply_brake > 0 @@ -53,13 +73,11 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ values["COMPUTER_BRAKE"] = apply_brake values["BRAKE_PUMP_REQUEST"] = pump_on - bus = get_pt_bus(car_fingerprint) - return packer.make_can_msg("BRAKE_COMMAND", bus, values) + return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values) -def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, car_fingerprint): +def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, car_fingerprint): commands = [] - bus = get_pt_bus(car_fingerprint) min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0] control_on = 5 if enabled else 0 @@ -96,43 +114,43 @@ def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, c "SET_TO_75": 0x75, "SET_TO_30": 0x30, } - commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values)) + commands.append(packer.make_can_msg("ACC_CONTROL_ON", CAN.pt, acc_control_on_values)) - commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values)) + commands.append(packer.make_can_msg("ACC_CONTROL", CAN.pt, acc_control_values)) return commands -def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, radar_disabled): +def create_steering_control(packer, CAN, apply_steer, lkas_active, car_fingerprint, radar_disabled): values = { "STEER_TORQUE": apply_steer if lkas_active else 0, "STEER_TORQUE_REQUEST": lkas_active, } - bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled) + bus = get_lkas_cmd_bus(CAN, car_fingerprint, radar_disabled) return packer.make_can_msg("STEERING_CONTROL", bus, values) -def create_bosch_supplemental_1(packer, car_fingerprint): +def create_bosch_supplemental_1(packer, CAN, car_fingerprint): # non-active params values = { "SET_ME_X04": 0x04, "SET_ME_X80": 0x80, "SET_ME_X10": 0x10, } - bus = get_lkas_cmd_bus(car_fingerprint) + bus = get_lkas_cmd_bus(CAN, car_fingerprint) return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) -def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active): +def create_ui_commands(packer, CAN, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active): commands = [] - bus_pt = get_pt_bus(CP.carFingerprint) radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl - bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled) + bus_lkas = get_lkas_cmd_bus(CAN, CP.carFingerprint, radar_disabled) if CP.openpilotLongitudinalControl: acc_hud_values = { 'CRUISE_SPEED': hud.v_cruise, 'ENABLE_MINI_CAR': 1 if enabled else 0, - 'HUD_DISTANCE': hud.personality_profile, + # only moves the lead car without ACC_ON + 'HUD_DISTANCE': (hud.lead_distance_bars + 1) % 4, # wraps to 0 at 4 bars 'IMPERIAL_UNIT': int(not is_metric), 'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0, 'SET_ME_X01_2': 1, @@ -143,6 +161,8 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, acc_hud_values['FCM_OFF'] = 1 acc_hud_values['FCM_OFF_2'] = 1 else: + # Shows the distance bars, TODO: stock camera shows updates temporarily while disabled + acc_hud_values['ACC_ON'] = int(enabled) acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH acc_hud_values['PCM_GAS'] = hud.pcm_accel acc_hud_values['SET_ME_X01'] = 1 @@ -150,7 +170,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2'] acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM'] acc_hud_values['ICONS'] = acc_hud['ICONS'] - commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values)) + commands.append(packer.make_can_msg("ACC_HUD", CAN.pt, acc_hud_values)) lkas_hud_values = { 'SET_ME_X41': 0x41, @@ -179,19 +199,19 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, 'CMBS_OFF': 0x01, 'SET_TO_1': 0x01, } - commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values)) + commands.append(packer.make_can_msg('RADAR_HUD', CAN.pt, radar_hud_values)) if CP.carFingerprint == CAR.CIVIC_BOSCH: - commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {})) + commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", CAN.pt, {})) return commands -def spam_buttons_command(packer, button_val, car_fingerprint): +def spam_buttons_command(packer, CAN, button_val, car_fingerprint): values = { 'CRUISE_BUTTONS': button_val, 'CRUISE_SETTING': 0, } # send buttons to camera on radarless cars - bus = 2 if car_fingerprint in HONDA_BOSCH_RADARLESS else get_pt_bus(car_fingerprint) + bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt return packer.make_can_msg("SCM_BUTTONS", bus, values) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 28db6a2..75de2c0 100644 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp -from openpilot.selfdrive.car.honda.hondacan import get_pt_bus -from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, \ - HONDA_BOSCH_RADARLESS +from openpilot.selfdrive.car.honda.hondacan import CanBus +from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CruiseSettings, HondaFlags, CAR, HONDA_BOSCH, \ + HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.disable_ecu import disable_ecu @@ -16,6 +16,8 @@ EventName = car.CarEvent.EventName TransmissionType = car.CarParams.TransmissionType BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} +SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.altButton1} +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -39,9 +41,11 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "honda" + CAN = CanBus(ret, fingerprint) + if candidate in HONDA_BOSCH: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] ret.radarUnavailable = True @@ -49,24 +53,24 @@ class CarInterface(CarInterfaceBase): # WARNING: THIS DISABLES AEB! # If Bosch radarless, this blocks ACC messages from the camera ret.experimentalLongitudinalAvailable = True - ret.openpilotLongitudinalControl = experimental_long and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = experimental_long ret.pcmCruise = not ret.openpilotLongitudinalControl else: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)] - ret.enableGasInterceptor = 0x201 in fingerprint[0] - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.enableGasInterceptor = 0x201 in fingerprint[CAN.pt] + ret.openpilotLongitudinalControl = not disable_openpilot_long ret.pcmCruise = not ret.enableGasInterceptor if candidate == CAR.CRV_5G: - ret.enableBsm = 0x12f8bfa7 in fingerprint[0] + ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar] # Detect Bosch cars with new HUD msgs if any(0x33DA in f for f in fingerprint.values()): ret.flags |= HondaFlags.BOSCH_EXT_HUD.value - # Accord 1.5T CVT has different gearbox message - if candidate == CAR.ACCORD and 0x191 in fingerprint[1]: + # Accord ICE 1.5T CVT has different gearbox message + if candidate == CAR.ACCORD and 0x191 in fingerprint[CAN.pt]: ret.transmissionType = TransmissionType.cvt # Certain Hondas have an extra steering sensor at the bottom of the steering rack, @@ -96,10 +100,6 @@ class CarInterface(CarInterfaceBase): eps_modified = True if candidate == CAR.CIVIC: - ret.mass = 1326. - ret.wheelbase = 2.70 - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 15.38 # 10.93 is end-to-end spec if eps_modified: # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 @@ -114,24 +114,15 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022): - ret.mass = 1326. - ret.wheelbase = 2.70 - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 15.38 # 10.93 is end-to-end spec if eps_modified: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] # 2.5x Modded EPS else: - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] - elif candidate in (CAR.ACCORD, CAR.ACCORDH): - ret.mass = 3279. * CV.LB_TO_KG - ret.wheelbase = 2.83 - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 16.33 # 11.82 is spec end-to-end + elif candidate == CAR.ACCORD: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.8467 if eps_modified: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] @@ -139,29 +130,15 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.ACURA_ILX: - ret.mass = 3095. * CV.LB_TO_KG - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.37 - ret.steerRatio = 18.61 # 15.3 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.72 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate in (CAR.CRV, CAR.CRV_EU): - ret.mass = 3572. * CV.LB_TO_KG - ret.wheelbase = 2.62 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.89 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.wheelSpeedFactor = 1.025 elif candidate == CAR.CRV_5G: - ret.mass = 3410. * CV.LB_TO_KG - ret.wheelbase = 2.66 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.0 # 12.3 is spec end-to-end if eps_modified: # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 @@ -171,45 +148,23 @@ class CarInterface(CarInterfaceBase): else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] - ret.tireStiffnessFactor = 0.677 ret.wheelSpeedFactor = 1.025 elif candidate == CAR.CRV_HYBRID: - ret.mass = 1667. # mean of 4 models in kg - ret.wheelbase = 2.66 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.0 # 12.3 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.wheelSpeedFactor = 1.025 elif candidate == CAR.FIT: - ret.mass = 2644. * CV.LB_TO_KG - ret.wheelbase = 2.53 - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 13.06 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] elif candidate == CAR.FREED: - ret.mass = 3086. * CV.LB_TO_KG - ret.wheelbase = 2.74 - # the remaining parameters were copied from FIT - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 13.06 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] - ret.tireStiffnessFactor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] elif candidate in (CAR.HRV, CAR.HRV_3G): - ret.mass = 3125 * CV.LB_TO_KG - ret.wheelbase = 2.61 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.2 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] - ret.tireStiffnessFactor = 0.5 if candidate == CAR.HRV: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] ret.wheelSpeedFactor = 1.025 @@ -217,29 +172,14 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning elif candidate == CAR.ACURA_RDX: - ret.mass = 3935. * CV.LB_TO_KG - ret.wheelbase = 2.68 - ret.centerToFront = ret.wheelbase * 0.38 - ret.steerRatio = 15.0 # as spec - ret.tireStiffnessFactor = 0.444 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate == CAR.ACURA_RDX_3G: - ret.mass = 4068. * CV.LB_TO_KG - ret.wheelbase = 2.75 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 11.95 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] - ret.tireStiffnessFactor = 0.677 elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): - ret.mass = 1900. - ret.wheelbase = 3.00 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 14.35 # as spec - ret.tireStiffnessFactor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] if candidate == CAR.ODYSSEY_CHN: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end @@ -247,47 +187,22 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end elif candidate == CAR.PILOT: - ret.mass = 4278. * CV.LB_TO_KG # average weight - ret.wheelbase = 2.86 - ret.centerToFront = ret.wheelbase * 0.428 - ret.steerRatio = 16.0 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.RIDGELINE: - ret.mass = 4515. * CV.LB_TO_KG - ret.wheelbase = 3.18 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.59 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.INSIGHT: - ret.mass = 2987. * CV.LB_TO_KG - ret.wheelbase = 2.7 - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 15.0 # 12.58 is spec end-to-end ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.HONDA_E: - ret.mass = 3338.8 * CV.LB_TO_KG - ret.wheelbase = 2.5 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 16.71 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning elif candidate == CAR.CLARITY: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_CLARITY - ret.mass = 4052. * CV.LB_TO_KG - ret.wheelbase = 2.75 - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 16.50 # 12.72 is end-to-end spec if eps_modified: for fw in car_fw: if fw.ecu == "eps" and b"-" not in fw.fwVersion and b"," in fw.fwVersion: @@ -300,14 +215,16 @@ class CarInterface(CarInterfaceBase): else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] - tire_stiffness_factor = 1. else: raise ValueError(f"unsupported car {candidate}") # These cars use alternate user brake msg (0x1BE) - if 0x1BE in fingerprint[get_pt_bus(candidate)] and candidate in HONDA_BOSCH: + # TODO: Only detect feature for Accord/Accord Hybrid, not all Bosch DBCs have BRAKE_MODULE + if 0x1BE in fingerprint[CAN.pt] and candidate == CAR.ACCORD: ret.flags |= HondaFlags.BOSCH_ALT_BRAKE.value + + if ret.flags & HondaFlags.BOSCH_ALT_BRAKE: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE # These cars use alternate SCM messages (SCM_FEEDBACK AND SCM_BUTTON) @@ -345,11 +262,12 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), - *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}), + *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] # events - events = self.create_common_events(ret, frogpilot_variables, pcm_enable=False) + events = self.create_common_events(ret, pcm_enable=False) if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.belowEngageSpeed) diff --git a/selfdrive/car/honda/tests/__init__.py b/selfdrive/car/honda/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/car/honda/tests/test_honda.py b/selfdrive/car/honda/tests/test_honda.py new file mode 100644 index 0000000..4e3f918 --- /dev/null +++ b/selfdrive/car/honda/tests/test_honda.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import re +import unittest + +from openpilot.selfdrive.car.honda.fingerprints import FW_VERSIONS + +HONDA_FW_VERSION_RE = br"\d{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$" + + +class TestHondaFingerprint(unittest.TestCase): + def test_fw_version_format(self): + # Asserts all FW versions follow an expected format + for fw_by_ecu in FW_VERSIONS.values(): + for fws in fw_by_ecu.values(): + for fw in fws: + self.assertTrue(re.match(HONDA_FW_VERSION_RE, fw) is not None, fw) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index e3f8e57..7c1471d 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -1,12 +1,11 @@ from dataclasses import dataclass -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Optional, Union +from enum import Enum, IntFlag from cereal import car from openpilot.common.conversions import Conversions as CV from panda.python import uds -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -49,10 +48,19 @@ class CarControllerParams: class HondaFlags(IntFlag): + # Detected flags # Bosch models with alternate set of LKAS_HUD messages BOSCH_EXT_HUD = 1 BOSCH_ALT_BRAKE = 2 + # Static flags + BOSCH = 4 + BOSCH_RADARLESS = 8 + + NIDEC = 16 + NIDEC_ALT_PCM_ACCEL = 32 + NIDEC_ALT_SCM_MESSAGES = 64 + # Car button codes class CruiseButtons: @@ -62,6 +70,11 @@ class CruiseButtons: MAIN = 1 +class CruiseSettings: + DISTANCE = 3 + LKAS = 1 + + # See dbc files for info on values VISUAL_HUD = { VisualAlert.none: 0, @@ -75,31 +88,15 @@ VISUAL_HUD = { } -class CAR(StrEnum): - ACCORD = "HONDA ACCORD 2018" - ACCORDH = "HONDA ACCORD HYBRID 2018" - CIVIC = "HONDA CIVIC 2016" - CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019" - CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019" - CIVIC_2022 = "HONDA CIVIC 2022" - ACURA_ILX = "ACURA ILX 2016" - CRV = "HONDA CR-V 2016" - CRV_5G = "HONDA CR-V 2017" - CRV_EU = "HONDA CR-V EU 2016" - CRV_HYBRID = "HONDA CR-V HYBRID 2019" - FIT = "HONDA FIT 2018" - FREED = "HONDA FREED 2020" - HRV = "HONDA HRV 2019" - HRV_3G = "HONDA HR-V 2023" - ODYSSEY = "HONDA ODYSSEY 2018" - ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019" - ACURA_RDX = "ACURA RDX 2018" - ACURA_RDX_3G = "ACURA RDX 2020" - PILOT = "HONDA PILOT 2017" - RIDGELINE = "HONDA RIDGELINE 2017" - INSIGHT = "HONDA INSIGHT 2019" - HONDA_E = "HONDA E 2020" - CLARITY = "HONDA CLARITY 2018" +@dataclass +class HondaCarDocs(CarDocs): + package: str = "Honda Sensing" + + def init_make(self, CP: car.CarParams): + if CP.flags & HondaFlags.BOSCH: + self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.flags & HondaFlags.BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a]) + else: + self.car_parts = CarParts.common([CarHarness.nidec]) class Footnote(Enum): @@ -108,56 +105,191 @@ class Footnote(Enum): Column.FSR_STEERING) -@dataclass -class HondaCarInfo(CarInfo): - package: str = "Honda Sensing" - - def init_make(self, CP: car.CarParams): - if CP.carFingerprint in HONDA_BOSCH: - self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a]) - else: - self.car_parts = CarParts.common([CarHarness.nidec]) +class HondaBoschPlatformConfig(PlatformConfig): + def init(self): + self.flags |= HondaFlags.BOSCH -CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { - CAR.ACCORD: [ - HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), - HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), - ], - CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"), - CAR.CIVIC_BOSCH: [ - HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", - footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS), - HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), - ], - CAR.CIVIC_BOSCH_DIESEL: None, # same platform - CAR.CIVIC_2022: [ - HondaCarInfo("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), - HondaCarInfo("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), - ], - CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS), - CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring - CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-20", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"), - CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"), - CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey - CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.PILOT: [ - HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - HondaCarInfo("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), - ], - CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-24", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.CLARITY: HondaCarInfo("Honda Clarity 2018-22"), -} +class HondaNidecPlatformConfig(PlatformConfig): + def init(self): + self.flags |= HondaFlags.NIDEC + + +class CAR(Platforms): + # Bosch Cars + ACCORD = HondaBoschPlatformConfig( + "HONDA ACCORD 2018", + [ + HondaCarDocs("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), + HondaCarDocs("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), + HondaCarDocs("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), + ], + # steerRatio: 11.82 is spec end-to-end + CarSpecs(mass=3279 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.33, centerToFrontRatio=0.39, tireStiffnessFactor=0.8467), + dbc_dict('honda_accord_2018_can_generated', None), + ) + CIVIC_BOSCH = HondaBoschPlatformConfig( + "HONDA CIVIC (BOSCH) 2019", + [ + HondaCarDocs("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", + footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS), + HondaCarDocs("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), + ], + CarSpecs(mass=1326, wheelbase=2.7, steerRatio=15.38, centerToFrontRatio=0.4), # steerRatio: 10.93 is end-to-end spec + dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), + ) + CIVIC_BOSCH_DIESEL = HondaBoschPlatformConfig( + "HONDA CIVIC SEDAN 1.6 DIESEL 2019", + [], # don't show in docs + CIVIC_BOSCH.specs, + dbc_dict('honda_accord_2018_can_generated', None), + ) + CIVIC_2022 = HondaBoschPlatformConfig( + "HONDA CIVIC 2022", + [ + HondaCarDocs("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), + HondaCarDocs("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), + ], + CIVIC_BOSCH.specs, + dbc_dict('honda_civic_ex_2022_can_generated', None), + flags=HondaFlags.BOSCH_RADARLESS, + ) + CRV_5G = HondaBoschPlatformConfig( + "HONDA CR-V 2017", + [HondaCarDocs("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)], + # steerRatio: 12.3 is spec end-to-end + CarSpecs(mass=3410 * CV.LB_TO_KG, wheelbase=2.66, steerRatio=16.0, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), + dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'), + flags=HondaFlags.BOSCH_ALT_BRAKE, + ) + CRV_HYBRID = HondaBoschPlatformConfig( + "HONDA CR-V HYBRID 2019", + [HondaCarDocs("Honda CR-V Hybrid 2017-20", min_steer_speed=12. * CV.MPH_TO_MS)], + # mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end + CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), + dbc_dict('honda_accord_2018_can_generated', None), + ) + HRV_3G = HondaBoschPlatformConfig( + "HONDA HR-V 2023", + [HondaCarDocs("Honda HR-V 2023", "All")], + CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.61, steerRatio=15.2, centerToFrontRatio=0.41, tireStiffnessFactor=0.5), + dbc_dict('honda_civic_ex_2022_can_generated', None), + flags=HondaFlags.BOSCH_RADARLESS | HondaFlags.BOSCH_ALT_BRAKE, + ) + ACURA_RDX_3G = HondaBoschPlatformConfig( + "ACURA RDX 2020", + [HondaCarDocs("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=4068 * CV.LB_TO_KG, wheelbase=2.75, steerRatio=11.95, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), # as spec + dbc_dict('acura_rdx_2020_can_generated', None), + flags=HondaFlags.BOSCH_ALT_BRAKE, + ) + INSIGHT = HondaBoschPlatformConfig( + "HONDA INSIGHT 2019", + [HondaCarDocs("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.0, centerToFrontRatio=0.39, tireStiffnessFactor=0.82), # as spec + dbc_dict('honda_insight_ex_2019_can_generated', None), + ) + HONDA_E = HondaBoschPlatformConfig( + "HONDA E 2020", + [HondaCarDocs("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=3338.8 * CV.LB_TO_KG, wheelbase=2.5, centerToFrontRatio=0.5, steerRatio=16.71, tireStiffnessFactor=0.82), + dbc_dict('acura_rdx_2020_can_generated', None), + ) + CLARITY = HondaBoschPlatformConfig( + "HONDA CLARITY 2018", + [HondaCarDocs("Honda Clarity 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=4052. * CV.LB_TO_KG, wheelbase=2.75, centerToFrontRatio=0.41, steerRatio=16.50, tireStiffnessFactor=1.), + dbc_dict('honda_clarity_hybrid_2018_can_generated', 'acura_ilx_2016_nidec'), + ) + + # Nidec Cars + ACURA_ILX = HondaNidecPlatformConfig( + "ACURA ILX 2016", + [HondaCarDocs("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS)], + CarSpecs(mass=3095 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=18.61, centerToFrontRatio=0.37, tireStiffnessFactor=0.72), # 15.3 is spec end-to-end + dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + CRV = HondaNidecPlatformConfig( + "HONDA CR-V 2016", + [HondaCarDocs("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=3572 * CV.LB_TO_KG, wheelbase=2.62, steerRatio=16.89, centerToFrontRatio=0.41, tireStiffnessFactor=0.444), # as spec + dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + CRV_EU = HondaNidecPlatformConfig( + "HONDA CR-V EU 2016", + [], # Euro version of CRV Touring, don't show in docs + CRV.specs, + dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + FIT = HondaNidecPlatformConfig( + "HONDA FIT 2018", + [HondaCarDocs("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=2644 * CV.LB_TO_KG, wheelbase=2.53, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), + dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + FREED = HondaNidecPlatformConfig( + "HONDA FREED 2020", + [HondaCarDocs("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=3086. * CV.LB_TO_KG, wheelbase=2.74, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), # mostly copied from FIT + dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HRV = HondaNidecPlatformConfig( + "HONDA HRV 2019", + [HondaCarDocs("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS)], + HRV_3G.specs, + dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + ODYSSEY = HondaNidecPlatformConfig( + "HONDA ODYSSEY 2018", + [HondaCarDocs("Honda Odyssey 2018-20")], + CarSpecs(mass=1900, wheelbase=3.0, steerRatio=14.35, centerToFrontRatio=0.41, tireStiffnessFactor=0.82), + dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_PCM_ACCEL, + ) + ODYSSEY_CHN = HondaNidecPlatformConfig( + "HONDA ODYSSEY CHN 2019", + [], # Chinese version of Odyssey, don't show in docs + ODYSSEY.specs, + dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + ACURA_RDX = HondaNidecPlatformConfig( + "ACURA RDX 2018", + [HondaCarDocs("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=3925 * CV.LB_TO_KG, wheelbase=2.68, steerRatio=15.0, centerToFrontRatio=0.38, tireStiffnessFactor=0.444), # as spec + dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + PILOT = HondaNidecPlatformConfig( + "HONDA PILOT 2017", + [ + HondaCarDocs("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarDocs("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), + ], + CarSpecs(mass=4278 * CV.LB_TO_KG, wheelbase=2.86, centerToFrontRatio=0.428, steerRatio=16.0, tireStiffnessFactor=0.444), # as spec + dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + RIDGELINE = HondaNidecPlatformConfig( + "HONDA RIDGELINE 2017", + [HondaCarDocs("Honda Ridgeline 2017-24", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=4515 * CV.LB_TO_KG, wheelbase=3.18, centerToFrontRatio=0.41, steerRatio=15.59, tireStiffnessFactor=0.444), # as spec + dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + CIVIC = HondaNidecPlatformConfig( + "HONDA CIVIC 2016", + [HondaCarDocs("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE")], + CarSpecs(mass=1326, wheelbase=2.70, centerToFrontRatio=0.4, steerRatio=15.38), # 10.93 is end-to-end spec + dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + ) + HONDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(0xF112) @@ -193,7 +325,6 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.UDS_VERSION_REQUEST], [StdQueries.UDS_VERSION_RESPONSE], bus=0, - logging=True, ), # Bosch PT bus Request( @@ -206,12 +337,16 @@ FW_QUERY_CONFIG = FwQueryConfig( # We lose these ECUs without the comma power on these cars. # Note that we still attempt to match with them when they are present non_essential_ecus={ - Ecu.programmedFuelInjection: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.transmission: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.vsa: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.combinationMeter: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.gateway: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.electricBrakeBooster: [CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.programmedFuelInjection: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.transmission: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.srs: [CAR.ACCORD], + Ecu.eps: [CAR.ACCORD], + Ecu.vsa: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.combinationMeter: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.gateway: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.electricBrakeBooster: [CAR.ACCORD, CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.shiftByWire: [CAR.ACCORD], # existence correlates with transmission type for ICE + Ecu.hud: [CAR.ACCORD], # existence correlates with trim level }, extra_ecus=[ # The only other ECU on PT bus accessible by camera on radarless Civic @@ -219,43 +354,16 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None), - CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None), - CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None), - CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), - CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_accord_2018_can_generated', None), - CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'), - CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.CRV_HYBRID: dbc_dict('honda_accord_2018_can_generated', None), - CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None), - CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), - CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), - CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), - CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None), - CAR.CIVIC_2022: dbc_dict('honda_civic_ex_2022_can_generated', None), - CAR.CLARITY: dbc_dict('honda_clarity_hybrid_2018_can_generated', 'acura_ilx_2016_nidec'), -} - STEER_THRESHOLD = { # default is 1200, overrides go here CAR.ACURA_RDX: 400, CAR.CRV_EU: 400, } -HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} -HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, - CAR.PILOT, CAR.RIDGELINE} -HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, - CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G} -HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G} +HONDA_NIDEC_ALT_PCM_ACCEL = CAR.with_flags(HondaFlags.NIDEC_ALT_PCM_ACCEL) +HONDA_NIDEC_ALT_SCM_MESSAGES = CAR.with_flags(HondaFlags.NIDEC_ALT_SCM_MESSAGES) +HONDA_BOSCH = CAR.with_flags(HondaFlags.BOSCH) +HONDA_BOSCH_RADARLESS = CAR.with_flags(HondaFlags.BOSCH_RADARLESS) + + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index fe2e8bb..a61da9f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -7,6 +7,7 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fau from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR +from openpilot.selfdrive.car.interfaces import CarControllerBase VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -42,7 +43,7 @@ def process_hud_alert(enabled, fingerprint, hud_control): return sys_warning, sys_state, left_lane_warning, right_lane_warning -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.CAN = CanBus(CP) @@ -131,13 +132,13 @@ class CarController: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, - set_speed_in_units, CS.personality_profile)) + set_speed_in_units, hud_control)) self.accel_last = accel else: # button presses can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False)) else: - can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req, + can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning)) @@ -150,8 +151,8 @@ class CarController: jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), - hud_control.leadVisible, set_speed_in_units, stopping, - CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile)) + hud_control, set_speed_in_units, stopping, + CC.cruiseControl.override, use_fca, CS.out.cruiseState.available)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: @@ -177,12 +178,12 @@ class CarController: can_sends = [] if use_clu11: if CC.cruiseControl.cancel: - can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint)) + can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) elif CC.cruiseControl.resume: # send resume at a max freq of 10Hz if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted - can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25) + can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP)] * 25) if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15: self.last_button_frame = self.frame else: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index da8ea3f..708bced 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -10,9 +10,6 @@ from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \ CANFD_CAR, Buttons, CarControllerParams from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS - -from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController PREV_BUTTON_SAMPLES = 8 CLUSTER_SAMPLE_RATE = 20 # frames @@ -183,61 +180,14 @@ class CarState(CarStateBase): if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: self.main_enabled = not self.main_enabled - # FrogPilot functions - distance_pressed = self.cruise_buttons[-1] == Buttons.GAP_DIST + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST - # Driving personalities function - if ret.cruiseState.available: - # Sync with the onroad UI button - if self.fpf.personality_changed_via_ui: - self.personality_profile = self.fpf.current_personality - self.fpf.reset_personality_changed_param() + if self.CP.flags & HyundaiFlags.CAN_LFA_BTN: + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] - if distance_pressed: - self.distance_pressed_counter += 1 - - elif self.distance_previously_pressed: - # Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds - if self.distance_pressed_counter < CRUISE_LONG_PRESS and frogpilot_variables.personalities_via_wheel: - self.personality_profile = (self.personality_profile + 2) % 3 - - self.fpf.distance_button_function(self.personality_profile) - - self.distance_pressed_counter = 0 - - # Switch the current state of Experimental Mode if the button is held down for 0.5 seconds - if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() - - # Switch the current state of Traffic Mode if the button is held down for 2.5 seconds - if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: - self.fpf.update_traffic_mode() - - # Revert the previous changes to Experimental Mode - if frogpilot_variables.experimental_mode_via_distance: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() - - self.distance_previously_pressed = distance_pressed - - # Toggle Experimental Mode from steering wheel function - if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.flags & HyundaiFlags.CAN_LFA_BTN: - lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] - - if lkas_pressed and not self.lkas_previously_pressed: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_lkas() - else: - self.fpf.update_experimental_mode() - self.lkas_previously_pressed = lkas_pressed - - SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv - SpeedLimitController.write_car_state() + self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv) return ret @@ -324,61 +274,13 @@ class CarState(CarStateBase): self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else cp_cam.vl["CAM_0x2a4"]) - # FrogPilot functions - distance_pressed = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 - # Driving personalities function - if ret.cruiseState.available: - # Sync with the onroad UI button - if self.fpf.personality_changed_via_ui: - self.personality_profile = self.fpf.current_personality - self.fpf.reset_personality_changed_param() + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] - if distance_pressed: - self.distance_pressed_counter += 1 - - elif self.distance_previously_pressed: - # Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds - if self.distance_pressed_counter < CRUISE_LONG_PRESS and frogpilot_variables.personalities_via_wheel: - self.personality_profile = (self.personality_profile + 2) % 3 - - self.fpf.distance_button_function(self.personality_profile) - - self.distance_pressed_counter = 0 - - # Switch the current state of Experimental Mode if the button is held down for 0.5 seconds - if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() - - # Switch the current state of Traffic Mode if the button is held down for 2.5 seconds - if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: - self.fpf.update_traffic_mode() - - # Revert the previous changes to Experimental Mode - if frogpilot_variables.experimental_mode_via_distance: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() - - self.distance_previously_pressed = distance_pressed - - # Toggle Experimental Mode from steering wheel function - if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available: - lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] - - if lkas_pressed and not self.lkas_previously_pressed: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_lkas() - else: - self.fpf.update_experimental_mode() - self.lkas_previously_pressed = lkas_pressed - - SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor - SpeedLimitController.write_car_state() + self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor) return ret diff --git a/selfdrive/car/hyundai/fingerprints.py b/selfdrive/car/hyundai/fingerprints.py index d1fc1fa..4116c65 100644 --- a/selfdrive/car/hyundai/fingerprints.py +++ b/selfdrive/car/hyundai/fingerprints.py @@ -5,21 +5,6 @@ from openpilot.selfdrive.car.hyundai.values import CAR Ecu = car.CarParams.Ecu FINGERPRINTS = { - CAR.HYUNDAI_GENESIS: [{ - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1268: 8, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1437: 8, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 - }], CAR.SANTA_FE: [{ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 }, @@ -32,33 +17,15 @@ FINGERPRINTS = { CAR.SONATA: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 549: 8, 550: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1096: 8, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 }], - CAR.SONATA_LF: [{ - 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], - CAR.KIA_SORENTO: [{ - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 - }], CAR.KIA_STINGER: [{ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8 }], - CAR.GENESIS_G80: [{ - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1024: 2, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8 - }, - { - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 546: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1437: 8, 1456: 4, 1470: 8 - }, - { - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1162: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1193: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4, 1470: 8 - }], CAR.GENESIS_G90: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1162: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2003: 8, 2004: 8, 2005: 8, 2008: 8, 2011: 8, 2012: 8, 2013: 8 }], CAR.IONIQ_EV_2020: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 }], - CAR.IONIQ: [{ - 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 8, 576: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1473: 8, 1476: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 - }], CAR.KONA_EV: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 1157: 4, 1193: 8, 1379: 8, 1988: 8, 1996: 8 }], @@ -74,9 +41,6 @@ FINGERPRINTS = { { 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 }], - CAR.PALISADE: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 547: 8, 548: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8 - }], } FW_VERSIONS = { @@ -100,15 +64,18 @@ FW_VERSIONS = { CAR.AZERA_HEV_6TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.00 99211-G8000 180903', + b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.01 99211-G8000 181109', b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.02 99211-G8100 191029', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00IG MDPS C 1.00 1.00 56310M9600\x00 4IHSC100', b'\xf1\x00IG MDPS C 1.00 1.01 56310M9350\x00 4IH8C101', + b'\xf1\x00IG MDPS C 1.00 1.02 56310M9350\x00 4IH8C102', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00IGhe SCC FHCUP 1.00 1.00 99110-M9100 ', b'\xf1\x00IGhe SCC FHCUP 1.00 1.01 99110-M9000 ', + b'\xf1\x00IGhe SCC FHCUP 1.00 1.02 99110-M9000 ', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x006T7N0_C2\x00\x006T7Q2051\x00\x00TIG2H24KA2\x12@\x11\xb7', @@ -413,6 +380,7 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', + b'\xf1\x006T6H0_C2\x00\x006T6B7051\x00\x00TLF0G24SL4;\x08\x12i', b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2\x00\x00\x00\x00', b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', b'\xf1\x87LAJSG49645724HF0\x87x\x87\x88\x87www\x88\x99\xa8\x89\x88\x99\xa8\x89\x88\x99\xa8\x89S_\xfb\xff\x87f\x7f\xff^2\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', @@ -567,25 +535,31 @@ FW_VERSIONS = { CAR.SANTA_FE_HEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', + b'\xf1\x00TMhe SCC FHCUP 1.00 1.01 99110-CL500 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLEC0 4TSHC102', b'\xf1\x00TM MDPS C 1.00 1.02 56310-GA000 4TSHA100', b'\xf1\x00TM MDPS R 1.00 1.05 57700-CL000 4TSHP105', + b'\xf1\x00TM MDPS R 1.00 1.06 57700-CL000 4TSHP106', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TMA MFC AT USA LHD 1.00 1.03 99211-S2500 220414', b'\xf1\x00TMH MFC AT EUR LHD 1.00 1.06 99211-S1500 220727', + b'\xf1\x00TMH MFC AT KOR LHD 1.00 1.06 99211-S1500 220727', b'\xf1\x00TMH MFC AT USA LHD 1.00 1.03 99211-S1500 210224', + b'\xf1\x00TMH MFC AT USA LHD 1.00 1.05 99211-S1500 220126', b'\xf1\x00TMH MFC AT USA LHD 1.00 1.06 99211-S1500 220727', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16KA1\xc6\x15Q\x1e', b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16SA3\xa3\x1b\xe14', b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16UA3I\x94\xac\x8f', b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TTM2H16SA2\x80\xd7l\xb2', ], (Ecu.engine, 0x7e0, None): [ + b'\xf1\x87391312MTA0', b'\xf1\x87391312MTC1', b'\xf1\x87391312MTE0', b'\xf1\x87391312MTL0', @@ -593,6 +567,7 @@ FW_VERSIONS = { }, CAR.SANTA_FE_PHEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00TMhe SCC F-CUP 1.00 1.00 99110-CL500 ', b'\xf1\x00TMhe SCC FHCUP 1.00 1.01 99110-CL500 ', b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', ], @@ -606,6 +581,7 @@ FW_VERSIONS = { b'\xf1\x00TMP MFC AT USA LHD 1.00 1.06 99211-S1500 220727', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA0o\x88^\xbe', b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea', b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA0o\x88^\xbe', b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea', @@ -724,6 +700,7 @@ FW_VERSIONS = { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360', b'\xf1\x00LX ESC \x01 1031\t\x10 58910-S8360', + b'\xf1\x00LX ESC \x01 104 \x10\x16 58910-S8360', b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330', b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', b'\xf1\x00LX ESC \x0b 103\x19\t\x07 58910-S8330', @@ -745,10 +722,12 @@ FW_VERSIONS = { b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8000 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.04 56310-S8020 4LXDC104', + b'\xf1\x00LX2 MDPS R 1.00 1.02 56370-S8300 9318', b'\xf1\x00ON MDPS C 1.00 1.00 56340-S9000 8B13', b'\xf1\x00ON MDPS C 1.00 1.01 56340-S9000 9201', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00LX2 MFC AT KOR LHD 1.00 1.08 99211-S8100 200903', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.00 99211-S8110 210226', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909', @@ -762,6 +741,7 @@ FW_VERSIONS = { b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', b'\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX2G38NB5X\xfa\xe88', b'\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00TON2G38NB5j\x94.\xde', b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1', b"\xf1\x87LBLUFN622950KF36\xa8\x88\x88\x88\x87w\x87xh\x99\x96\x89\x88\x99\x98\x89\x88\x99\x98\x89\x87o\xf6\xff\x98\x88o\xffx'\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8", @@ -864,10 +844,12 @@ FW_VERSIONS = { b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9420 4I4VL107', b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9200 4I2CL108', b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9420 4I4VL108', + b'\xf1\x00IK MDPS R 1.00 5.09 57700-G9520 4I4VL509', ], (Ecu.transmission, 0x7e1, None): [ b'\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T20KB3Wuvz', + b'\xf1\x00bcsh8p54 E31\x00\x00\x00\x00\x00\x00\x00SIK0T33NH0\x0f\xa3Y*', b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00', b'\xf1\x87VDJLC18480772DK9\x88eHfwfff\x87eFUeDEU\x98eFe\x86T5DVyo\xff\x87s\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33KB5\x9f\xa5&\x81', b'\xf1\x87VDKLT18912362DN4wfVfwefeveVUwfvw\x88vWfvUFU\x89\xa9\x8f\xff\x87w\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', @@ -875,21 +857,25 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02', + b'\xf1\x00IK__ SCC FHCUP 1.00 1.00 99110-G9300 ', b'\xf1\x00IK__ SCC FHCUP 1.00 1.02 96400-G9000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00IK MFC AT KOR LHD 1.00 1.01 95740-G9000 170920', b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', + b'\xf1\x00IK MFC AT USA LHD 1.00 1.04 99211-G9000 220401', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81606G2051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640N2051\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.GENESIS_G80: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DH__ SCC F-CUP 1.00 1.01 96400-B1120 ', + b'\xf1\x00DH__ SCC F-CUP 1.00 1.02 96400-B1120 ', b'\xf1\x00DH__ SCC FHCUP 1.00 1.01 96400-B1110 ', ], (Ecu.fwdCamera, 0x7c4, None): [ @@ -897,10 +883,12 @@ FW_VERSIONS = { b'\xf1\x00DH LKAS AT USA LHD 1.01 1.01 95895-B1500 161014', b'\xf1\x00DH LKAS AT USA LHD 1.01 1.02 95895-B1500 170810', b'\xf1\x00DH LKAS AT USA LHD 1.01 1.03 95895-B1500 180713', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.04 95895-B1500 181213', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G33KH2\xae\xde\xd5!', b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH2j\x9dA\x1c', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH3\xaf\x1a7\xe2', b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0T33NH3\x97\xe6\xbc\xb8', b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00TDH0G38NH3:-\xa9n', b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SDH0T33NH4\xd7O\x9e\xc9', @@ -912,16 +900,19 @@ FW_VERSIONS = { }, CAR.GENESIS_G90: { (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SHI0G50NH0\xff\x80\xc2*', b'\xf1\x87VDGMD15352242DD3w\x87gxwvgv\x87wvw\x88wXwffVfffUfw\x88o\xff\x06J\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7', b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7', b'\xf1\x87VDHMD16446682DD3WwwxxvGw\x88\x88\x87\x88\x88whxx\x87\x87\x87\x85fUfwu_\xffT\xf8\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 ', + b'\xf1\x00HI__ SCC FHCUP 1.00 1.02 99110-D2100 ', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00HI LKAS AT USA LHD 1.00 1.00 95895-D2020 160302', b'\xf1\x00HI LKAS AT USA LHD 1.00 1.00 95895-D2030 170208', + b'\xf1\x00HI MFC AT USA LHD 1.00 1.03 99211-D2000 190831', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x810000000000\x00', @@ -1089,6 +1080,7 @@ FW_VERSIONS = { ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00OS MDPS C 1.00 1.03 56310/K4550 4OEDC103', + b'\xf1\x00OS MDPS C 1.00 1.04 56310-XX000 4OEDC104', b'\xf1\x00OS MDPS C 1.00 1.04 56310K4000\x00 4OEDC104', b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ], @@ -1176,11 +1168,13 @@ FW_VERSIONS = { }, CAR.KIA_NIRO_PHEV: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6D0051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006U3H0_C2\x00\x006U3G0051\x00\x00HDE0G16NS2\x00\x00\x00\x00', b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL2&[\xc3\x01', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x13\xcd\x88\x92', @@ -1192,6 +1186,7 @@ FW_VERSIONS = { b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DEH MFC AT USA LHD 1.00 1.00 95740-G5010 170117', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.00 95740-G5010 170117', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.05 99211-G5000 190826', @@ -1476,11 +1471,13 @@ FW_VERSIONS = { CAR.SONATA_HYBRID: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DNhe SCC F-CUP 1.00 1.02 99110-L5000 ', + b'\xf1\x00DNhe SCC FHCUP 1.00 1.00 99110-L5000 ', b'\xf1\x00DNhe SCC FHCUP 1.00 1.02 99110-L5000 ', b'\xf1\x8799110L5000\xf1\x00DNhe SCC F-CUP 1.00 1.02 99110-L5000 ', b'\xf1\x8799110L5000\xf1\x00DNhe SCC FHCUP 1.00 1.02 99110-L5000 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L5000 4DNHC101', b'\xf1\x00DN8 MDPS C 1.00 1.03 56310L5450\x00 4DNHC104', b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102', b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.03 56310-L5450 4DNHC103', @@ -1488,12 +1485,14 @@ FW_VERSIONS = { b'\xf1\x8756310L5450\x00\xf1\x00DN8 MDPS C 1.00 1.03 56310L5450\x00 4DNHC104', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DN8HMFC AT KOR LHD 1.00 1.03 99211-L1000 190705', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.04 99211-L1000 191016', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.05 99211-L1000 201109', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.06 99211-L1000 210325', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.07 99211-L1000 211223', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00PSBG2314 E07\x00\x00\x00\x00\x00\x00\x00TDN2H20KA5\xba\x82\xc7\xc3', b'\xf1\x00PSBG2323 E09\x00\x00\x00\x00\x00\x00\x00TDN2H20SA5\x97R\x88\x9e', b'\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW', b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TDN2H20SA7\x1a3\xf9\xab', @@ -1503,6 +1502,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x87391062J002', + b'\xf1\x87391162J011', b'\xf1\x87391162J012', b'\xf1\x87391162J013', b'\xf1\x87391162J014', @@ -1510,15 +1510,18 @@ FW_VERSIONS = { }, CAR.KIA_SORENTO: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00UMP LKAS AT USA LHD 1.00 1.00 95740-C6550 d00', b'\xf1\x00UMP LKAS AT USA LHD 1.01 1.01 95740-C6550 d01', ], (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00UM ESC \x02 12 \x18\x05\x05 58910-C6300', b'\xf1\x00UM ESC \x0c 12 \x18\x05\x06 58910-C6330', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00UM__ SCC F-CUP 1.00 1.00 96400-C6500 ', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 U834\x00\x00\x00\x00\x00\x00TUM2G33NL7K\xae\xdd\x1d', b'\xf1\x87LDKUAA0348164HE3\x87www\x87www\x88\x88\xa8\x88w\x88\x97xw\x88\x97x\x86o\xf8\xff\x87f\x7f\xff\x15\xe0\xf1\x81U811\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U811\x00\x00\x00\x00\x00\x00TUM4G33NL3V|DG', ], (Ecu.engine, 0x7e0, None): [ @@ -1546,7 +1549,9 @@ FW_VERSIONS = { b'\xf1\x00NE1_ RDR ----- 1.00 1.00 99110-GI000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.01 99211-GI010 211007', b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', + b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI010 230110', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206', b'\xf1\x00NE1 MFC AT KOR LHD 1.00 1.00 99211-GI020 230719', @@ -1564,12 +1569,14 @@ FW_VERSIONS = { b'\xf1\x00CE__ RDR ----- 1.00 1.01 99110-KL000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CE MFC AT CAN LHD 1.00 1.04 99211-KL000 221213', b'\xf1\x00CE MFC AT EUR LHD 1.00 1.03 99211-KL000 221011', b'\xf1\x00CE MFC AT USA LHD 1.00 1.04 99211-KL000 221213', ], }, CAR.TUCSON_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.01 99211-N9100 14A', b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 1.00 99211-N9220 14K', b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 2.02 99211-N9000 14E', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9210 14G', @@ -1591,6 +1598,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW000 14M', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW010 14X', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW020 14Z', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-K5000 ', @@ -1604,6 +1612,7 @@ FW_VERSIONS = { b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1030 662', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1040 663', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1060 665', + b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1070 690', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ', diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index c8a6976..0e75cc1 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -1,9 +1,9 @@ import crcmod -from openpilot.selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR +from openpilot.selfdrive.car.hyundai.values import CAR, HyundaiFlags hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) -def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, +def create_lkas11(packer, frame, CP, apply_steer, steer_req, torque_fault, lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_depart, right_lane_depart): @@ -33,12 +33,12 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_ToiFlt"] = torque_fault # seems to allow actuation on CR_Lkas_StrToqReq values["CF_Lkas_MsgCount"] = frame % 0x10 - if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, - CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, - CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, - CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, - CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, - CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN): + if CP.carFingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, + CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, + CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, + CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, + CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, + CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 @@ -57,7 +57,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 # Likely cars lacking the ability to show individual lane lines in the dash - elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): + elif CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): # SysWarning 4 = keep hands on wheel + beep values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 @@ -72,18 +72,18 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_LdwsActivemode"] = 0 values["CF_Lkas_FcwOpt_USM"] = 0 - elif car_fingerprint == CAR.HYUNDAI_GENESIS: + elif CP.carFingerprint == CAR.HYUNDAI_GENESIS: # This field is actually LdwsActivemode # Genesis and Optima fault when forwarding while engaged values["CF_Lkas_LdwsActivemode"] = 2 dat = packer.make_can_msg("LKAS11", 0, values)[2] - if car_fingerprint in CHECKSUM["crc8"]: + if CP.flags & HyundaiFlags.CHECKSUM_CRC8: # CRC Checksum as seen on 2019 Hyundai Santa Fe dat = dat[:6] + dat[7:8] checksum = hyundai_checksum(dat) - elif car_fingerprint in CHECKSUM["6B"]: + elif CP.flags & HyundaiFlags.CHECKSUM_6B: # Checksum of first 6 Bytes, as seen on 2018 Kia Sorento checksum = sum(dat[:6]) % 256 else: @@ -95,7 +95,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, return packer.make_can_msg("LKAS11", 0, values) -def create_clu11(packer, frame, clu11, button, car_fingerprint): +def create_clu11(packer, frame, clu11, button, CP): values = {s: clu11[s] for s in [ "CF_Clu_CruiseSwState", "CF_Clu_CruiseSwMain", @@ -113,7 +113,7 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint): values["CF_Clu_CruiseSwState"] = button values["CF_Clu_AliveCnt1"] = frame % 0x10 # send buttons to camera on camera-scc based cars - bus = 2 if car_fingerprint in CAMERA_SCC_CAR else 0 + bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0 return packer.make_can_msg("CLU11", bus, values) @@ -126,12 +126,12 @@ def create_lfahda_mfc(packer, enabled, lat_active, hda_set_speed=0): } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available, personality_profile): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, cruise_available): commands = [] scc11_values = { "MainMode_ACC": 1 if cruise_available else 0, - "TauGapSet": personality_profile + 1, + "TauGapSet": hud_control.leadDistanceBars, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, "ObjValid": 1, # close lead makes controls tighter @@ -167,7 +167,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage - "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead + "ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead } commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 02ceca2..7e74ca8 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active): return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, personality_profile): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -146,7 +146,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov "SET_ME_2": 0x4, "SET_ME_3": 0x3, "SET_ME_TMP_64": 0x64, - "DISTANCE_SETTING": personality_profile + 1, + "DISTANCE_SETTING": hud_control.leadDistanceBars, } return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index f8a29b3..e17b7bb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,6 +1,5 @@ -from cereal import car +from cereal import car, custom from panda import Panda -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \ @@ -17,11 +16,12 @@ GearShifter = car.CarState.GearShifter ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -77,196 +77,6 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate in (CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN): - ret.mass = 1600. if candidate == CAR.AZERA_6TH_GEN else 1675. # ICE is ~average of 2.5L and 3.5L - ret.wheelbase = 2.885 - ret.steerRatio = 14.5 - elif candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): - ret.mass = 3982. * CV.LB_TO_KG - ret.wheelbase = 2.766 - # Values from optimizer - ret.steerRatio = 16.55 # 13.8 is spec end-to-end - ret.tireStiffnessFactor = 0.82 - elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): - ret.mass = 1513. - ret.wheelbase = 2.84 - ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.SONATA_LF: - ret.mass = 1536. - ret.wheelbase = 2.804 - ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - elif candidate == CAR.PALISADE: - ret.mass = 1999. - ret.wheelbase = 2.90 - ret.steerRatio = 15.6 * 1.15 - ret.tireStiffnessFactor = 0.63 - elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): - ret.mass = 1275. - ret.wheelbase = 2.7 - ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 - ret.tireStiffnessFactor = 0.385 # stiffnessFactor settled on 1.0081302973865127 - ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate == CAR.ELANTRA_2021: - ret.mass = 2800. * CV.LB_TO_KG - ret.wheelbase = 2.72 - ret.steerRatio = 12.9 - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.ELANTRA_HEV_2021: - ret.mass = 3017. * CV.LB_TO_KG - ret.wheelbase = 2.72 - ret.steerRatio = 12.9 - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.HYUNDAI_GENESIS: - ret.mass = 2060. - ret.wheelbase = 3.01 - ret.steerRatio = 16.5 - ret.minSteerSpeed = 60 * CV.KPH_TO_MS - elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_EV_2ND_GEN): - ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743., CAR.KONA_EV_2ND_GEN: 1740.}.get(candidate, 1275.) - ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6) - ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec - ret.tireStiffnessFactor = 0.385 - elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV): - ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx - ret.wheelbase = 2.7 - ret.steerRatio = 13.73 # Spec - ret.tireStiffnessFactor = 0.385 - if candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019): - ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6): - ret.mass = 1948 - ret.wheelbase = 2.97 - ret.steerRatio = 14.26 - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.VELOSTER: - ret.mass = 2917. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 * 1.15 - ret.tireStiffnessFactor = 0.5 - elif candidate == CAR.TUCSON: - ret.mass = 3520. * CV.LB_TO_KG - ret.wheelbase = 2.67 - ret.steerRatio = 14.00 * 1.15 - ret.tireStiffnessFactor = 0.385 - elif candidate == CAR.TUCSON_4TH_GEN: - ret.mass = 1630. # average - ret.wheelbase = 2.756 - ret.steerRatio = 16. - ret.tireStiffnessFactor = 0.385 - elif candidate == CAR.SANTA_CRUZ_1ST_GEN: - ret.mass = 1870. # weight from Limited trim - the only supported trim - ret.wheelbase = 3.000 - # steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf - ret.steerRatio = 14.2 - elif candidate == CAR.CUSTIN_1ST_GEN: - ret.mass = 1690. # from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0 - ret.wheelbase = 3.055 - ret.steerRatio = 17.0 # from learner - elif candidate == CAR.STARIA_4TH_GEN: - ret.mass = 2205. - ret.wheelbase = 3.273 - ret.steerRatio = 11.94 # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf - - # Kia - elif candidate == CAR.KIA_SORENTO: - ret.mass = 1985. - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_PHEV_2022): - ret.mass = 3543. * CV.LB_TO_KG # average of all the cars - ret.wheelbase = 2.7 - ret.steerRatio = 13.6 # average of all the cars - ret.tireStiffnessFactor = 0.385 - if candidate == CAR.KIA_NIRO_PHEV: - ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate == CAR.KIA_SELTOS: - ret.mass = 1337. - ret.wheelbase = 2.63 - ret.steerRatio = 14.56 - elif candidate == CAR.KIA_SPORTAGE_5TH_GEN: - ret.mass = 1725. # weight from SX and above trims, average of FWD and AWD versions - ret.wheelbase = 2.756 - ret.steerRatio = 13.6 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications - elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL): - ret.mass = 3558. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 - ret.tireStiffnessFactor = 0.5 - if candidate == CAR.KIA_OPTIMA_G4: - ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022): - ret.mass = 1825. - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - elif candidate == CAR.KIA_FORTE: - ret.mass = 2878. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 - ret.tireStiffnessFactor = 0.5 - elif candidate == CAR.KIA_CEED: - ret.mass = 1450. - ret.wheelbase = 2.65 - ret.steerRatio = 13.75 - ret.tireStiffnessFactor = 0.5 - elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020): - ret.mass = 3381. * CV.LB_TO_KG - ret.wheelbase = 2.85 - ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) - ret.tireStiffnessFactor = 0.5 - elif candidate == CAR.KIA_EV6: - ret.mass = 2055 - ret.wheelbase = 2.9 - ret.steerRatio = 16. - ret.tireStiffnessFactor = 0.65 - elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN): - ret.wheelbase = 2.81 - ret.steerRatio = 13.5 # average of the platforms - if candidate == CAR.KIA_SORENTO_4TH_GEN: - ret.mass = 3957 * CV.LB_TO_KG - else: - ret.mass = 4396 * CV.LB_TO_KG - elif candidate == CAR.KIA_CARNIVAL_4TH_GEN: - ret.mass = 2087. - ret.wheelbase = 3.09 - ret.steerRatio = 14.23 - elif candidate == CAR.KIA_K8_HEV_1ST_GEN: - ret.mass = 1630. # https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid - ret.wheelbase = 2.895 - ret.steerRatio = 13.27 # guesstimate from K5 platform - - # Genesis - elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN: - ret.mass = 2205 - ret.wheelbase = 2.9 - # https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1. - ret.steerRatio = 12.6 - elif candidate == CAR.GENESIS_G70: - ret.steerActuatorDelay = 0.1 - ret.mass = 1640.0 - ret.wheelbase = 2.84 - ret.steerRatio = 13.56 - elif candidate == CAR.GENESIS_G70_2020: - ret.mass = 3673.0 * CV.LB_TO_KG - ret.wheelbase = 2.83 - ret.steerRatio = 12.9 - elif candidate == CAR.GENESIS_GV70_1ST_GEN: - ret.mass = 1950. - ret.wheelbase = 2.87 - ret.steerRatio = 14.6 - elif candidate == CAR.GENESIS_G80: - ret.mass = 2060. - ret.wheelbase = 3.01 - ret.steerRatio = 16.5 - elif candidate == CAR.GENESIS_G90: - ret.mass = 2200. - ret.wheelbase = 3.15 - ret.steerRatio = 12.069 - elif candidate == CAR.GENESIS_GV80: - ret.mass = 2258. - ret.wheelbase = 2.95 - ret.steerRatio = 14.14 - # *** longitudinal control *** if candidate in CANFD_CAR: ret.longitudinalTuning.kpV = [0.1] @@ -276,7 +86,7 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kpV = [0.5] ret.longitudinalTuning.kiV = [0.0] ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR) - ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable ret.pcmCruise = not ret.openpilotLongitudinalControl ret.stoppingControl = True @@ -352,13 +162,16 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) if self.CS.CP.openpilotLongitudinalControl: - ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons # Main button also can trigger an engagement on these cars allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons) - events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.sport, GearShifter.manumatic], + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.manumatic], pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) diff --git a/selfdrive/car/hyundai/tests/__init__.py b/selfdrive/car/hyundai/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/car/hyundai/tests/print_platform_codes.py b/selfdrive/car/hyundai/tests/print_platform_codes.py new file mode 100644 index 0000000..f641535 --- /dev/null +++ b/selfdrive/car/hyundai/tests/print_platform_codes.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +from cereal import car +from openpilot.selfdrive.car.hyundai.values import PLATFORM_CODE_ECUS, get_platform_codes +from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +if __name__ == "__main__": + for car_model, ecus in FW_VERSIONS.items(): + print() + print(car_model) + for ecu in sorted(ecus, key=lambda x: int(x[0])): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + platform_codes = get_platform_codes(ecus[ecu]) + codes = {code for code, _ in platform_codes} + dates = {date for _, date in platform_codes if date is not None} + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + print(f' Codes: {codes}') + print(f' Dates: {dates}') diff --git a/selfdrive/car/hyundai/tests/test_hyundai.py b/selfdrive/car/hyundai/tests/test_hyundai.py new file mode 100644 index 0000000..61b11a1 --- /dev/null +++ b/selfdrive/car/hyundai/tests/test_hyundai.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python3 +from hypothesis import settings, given, strategies as st +import unittest + +from cereal import car +from openpilot.selfdrive.car.fw_versions import build_fw_dict +from openpilot.selfdrive.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \ + HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \ + UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ + get_platform_codes +from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +# Some platforms have date codes in a different format we don't yet parse (or are missing). +# For now, assert list of expected missing date cars +NO_DATES_PLATFORMS = { + # CAN FD + CAR.KIA_SPORTAGE_5TH_GEN, + CAR.SANTA_CRUZ_1ST_GEN, + CAR.TUCSON_4TH_GEN, + # CAN + CAR.ELANTRA, + CAR.ELANTRA_GT_I30, + CAR.KIA_CEED, + CAR.KIA_FORTE, + CAR.KIA_OPTIMA_G4, + CAR.KIA_OPTIMA_G4_FL, + CAR.KIA_SORENTO, + CAR.KONA, + CAR.KONA_EV, + CAR.KONA_EV_2022, + CAR.KONA_HEV, + CAR.SONATA_LF, + CAR.VELOSTER, +} + + +class TestHyundaiFingerprint(unittest.TestCase): + def test_can_features(self): + # Test no EV/HEV in any gear lists (should all use ELECT_GEAR) + self.assertEqual(set.union(*CAN_GEARS.values()) & (HYBRID_CAR | EV_CAR), set()) + + # Test CAN FD car not in CAN feature lists + can_specific_feature_list = set.union(*CAN_GEARS.values(), *CHECKSUM.values(), LEGACY_SAFETY_MODE_CAR, UNSUPPORTED_LONGITUDINAL_CAR, CAMERA_SCC_CAR) + for car_model in CANFD_CAR: + self.assertNotIn(car_model, can_specific_feature_list, "CAN FD car unexpectedly found in a CAN feature list") + + def test_hybrid_ev_sets(self): + self.assertEqual(HYBRID_CAR & EV_CAR, set(), "Shared cars between hybrid and EV") + self.assertEqual(CANFD_CAR & HYBRID_CAR, set(), "Hard coding CAN FD cars as hybrid is no longer supported") + + def test_auxiliary_request_ecu_whitelist(self): + # Asserts only auxiliary Ecus can exist in database for CAN-FD cars + whitelisted_ecus = {ecu for r in FW_QUERY_CONFIG.requests for ecu in r.whitelist_ecus if r.auxiliary} + + for car_model in CANFD_CAR: + ecus = {fw[0] for fw in FW_VERSIONS[car_model].keys()} + ecus_not_in_whitelist = ecus - whitelisted_ecus + ecu_strings = ", ".join([f"Ecu.{ECU_NAME[ecu]}" for ecu in ecus_not_in_whitelist]) + self.assertEqual(len(ecus_not_in_whitelist), 0, + f"{car_model}: Car model has ECUs not in auxiliary request whitelists: {ecu_strings}") + + def test_blacklisted_parts(self): + # Asserts no ECUs known to be shared across platforms exist in the database. + # Tucson having Santa Cruz camera and EPS for example + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + if car_model == CAR.SANTA_CRUZ_1ST_GEN: + raise unittest.SkipTest("Skip checking Santa Cruz for its parts") + + for code, _ in get_platform_codes(ecus[(Ecu.fwdCamera, 0x7c4, None)]): + if b"-" not in code: + continue + part = code.split(b"-")[1] + self.assertFalse(part.startswith(b'CW'), "Car has bad part number") + + def test_correct_ecu_response_database(self): + """ + Assert standard responses for certain ECUs, since they can + respond to multiple queries with different data + """ + expected_fw_prefix = HYUNDAI_VERSION_REQUEST_LONG[1:] + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for ecu, fws in ecus.items(): + # TODO: enable for Ecu.fwdRadar, Ecu.abs, Ecu.eps, Ecu.transmission + if ecu[0] in (Ecu.fwdCamera,): + self.assertTrue(all(fw.startswith(expected_fw_prefix) for fw in fws), + f"FW from unexpected request in database: {(ecu, fws)}") + + @settings(max_examples=100) + @given(data=st.data()) + def test_platform_codes_fuzzy_fw(self, data): + """Ensure function doesn't raise an exception""" + fw_strategy = st.lists(st.binary()) + fws = data.draw(fw_strategy) + get_platform_codes(fws) + + def test_expected_platform_codes(self): + # Ensures we don't accidentally add multiple platform codes for a car unless it is intentional + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for ecu, fws in ecus.items(): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + # Third and fourth character are usually EV/hybrid identifiers + codes = {code.split(b"-")[0][:2] for code, _ in get_platform_codes(fws)} + if car_model == CAR.PALISADE: + self.assertEqual(codes, {b"LX", b"ON"}, f"Car has unexpected platform codes: {car_model} {codes}") + elif car_model == CAR.KONA_EV and ecu[0] == Ecu.fwdCamera: + self.assertEqual(codes, {b"OE", b"OS"}, f"Car has unexpected platform codes: {car_model} {codes}") + else: + self.assertEqual(len(codes), 1, f"Car has multiple platform codes: {car_model} {codes}") + + # Tests for platform codes, part numbers, and FW dates which Hyundai will use to fuzzy + # fingerprint in the absence of full FW matches: + def test_platform_code_ecus_available(self): + # TODO: add queries for these non-CAN FD cars to get EPS + no_eps_platforms = CANFD_CAR | {CAR.KIA_SORENTO, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, + CAR.KIA_OPTIMA_H_G4_FL, CAR.SONATA_LF, CAR.TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.ELANTRA} + + # Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for platform_code_ecu in PLATFORM_CODE_ECUS: + if platform_code_ecu in (Ecu.fwdRadar, Ecu.eps) and car_model == CAR.HYUNDAI_GENESIS: + continue + if platform_code_ecu == Ecu.eps and car_model in no_eps_platforms: + continue + self.assertIn(platform_code_ecu, [e[0] for e in ecus]) + + def test_fw_format(self): + # Asserts: + # - every supported ECU FW version returns one platform code + # - every supported ECU FW version has a part number + # - expected parsing of ECU FW dates + + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for ecu, fws in ecus.items(): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + codes = set() + for fw in fws: + result = get_platform_codes([fw]) + self.assertEqual(1, len(result), f"Unable to parse FW: {fw}") + codes |= result + + if ecu[0] not in DATE_FW_ECUS or car_model in NO_DATES_PLATFORMS: + self.assertTrue(all(date is None for _, date in codes)) + else: + self.assertTrue(all(date is not None for _, date in codes)) + + if car_model == CAR.HYUNDAI_GENESIS: + raise unittest.SkipTest("No part numbers for car model") + + # Hyundai places the ECU part number in their FW versions, assert all parsable + # Some examples of valid formats: b"56310-L0010", b"56310L0010", b"56310/M6300" + self.assertTrue(all(b"-" in code for code, _ in codes), + f"FW does not have part number: {fw}") + + def test_platform_codes_spot_check(self): + # Asserts basic platform code parsing behavior for a few cases + results = get_platform_codes([b"\xf1\x00DH LKAS 1.1 -150210"]) + self.assertEqual(results, {(b"DH", b"150210")}) + + # Some cameras and all radars do not have dates + results = get_platform_codes([b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 "]) + self.assertEqual(results, {(b"AEhe-G2000", None)}) + + results = get_platform_codes([b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 "]) + self.assertEqual(results, {(b"CV1-CV000", None)}) + + results = get_platform_codes([ + b"\xf1\x00DH LKAS 1.1 -150210", + b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ", + b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ", + ]) + self.assertEqual(results, {(b"DH", b"150210"), (b"AEhe-G2000", None), (b"CV1-CV000", None)}) + + results = get_platform_codes([ + b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.07 99211-S8100 220222", + b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 211103", + b"\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 190405", + b"\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 190720", + ]) + self.assertEqual(results, {(b"LX2-S8100", b"220222"), (b"LX2-S8100", b"211103"), + (b"ON-S9100", b"190405"), (b"ON-S9100", b"190720")}) + + def test_fuzzy_excluded_platforms(self): + # Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared. + # This list can be shrunk as we combine platforms and detect features + excluded_platforms = { + CAR.GENESIS_G70, # shared platform code, part number, and date + CAR.GENESIS_G70_2020, + } + excluded_platforms |= CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST # shared platform codes + excluded_platforms |= NO_DATES_PLATFORMS # date codes are required to match + + platforms_with_shared_codes = set() + for platform, fw_by_addr in FW_VERSIONS.items(): + car_fw = [] + for ecu, fw_versions in fw_by_addr.items(): + ecu_name, addr, sub_addr = ecu + for fw in fw_versions: + car_fw.append({"ecu": ecu_name, "fwVersion": fw, "address": addr, + "subAddress": 0 if sub_addr is None else sub_addr}) + + CP = car.CarParams.new_message(carFw=car_fw) + matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), FW_VERSIONS) + if len(matches) == 1: + self.assertEqual(list(matches)[0], platform) + else: + platforms_with_shared_codes.add(platform) + + self.assertEqual(platforms_with_shared_codes, excluded_platforms) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index a742125..915a581 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1,13 +1,12 @@ import re -from dataclasses import dataclass -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Optional, Set, Tuple, Union +from dataclasses import dataclass, field +from enum import Enum, IntFlag from cereal import car from panda.python import uds from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -54,94 +53,50 @@ class CarControllerParams: class HyundaiFlags(IntFlag): + # Dynamic Flags CANFD_HDA2 = 1 CANFD_ALT_BUTTONS = 2 - CANFD_ALT_GEARS = 4 - CANFD_CAMERA_SCC = 8 + CANFD_ALT_GEARS = 2 ** 2 + CANFD_CAMERA_SCC = 2 ** 3 - ALT_LIMITS = 16 - ENABLE_BLINKERS = 32 - CANFD_ALT_GEARS_2 = 64 - SEND_LFA = 128 - USE_FCA = 256 - CANFD_HDA2_ALT_STEERING = 512 - HYBRID = 1024 - EV = 2048 - CAN_LFA_BTN = 4096 + ALT_LIMITS = 2 ** 4 + ENABLE_BLINKERS = 2 ** 5 + CANFD_ALT_GEARS_2 = 2 ** 6 + SEND_LFA = 2 ** 7 + USE_FCA = 2 ** 8 + CANFD_HDA2_ALT_STEERING = 2 ** 9 + # these cars use a different gas signal + HYBRID = 2 ** 10 + EV = 2 ** 11 -class CAR(StrEnum): - # Hyundai - AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN" - AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN" - ELANTRA = "HYUNDAI ELANTRA 2017" - ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" - ELANTRA_2021 = "HYUNDAI ELANTRA 2021" - ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021" - HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" - IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019" - IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022" - IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019" - IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020" - IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019" - IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020" - KONA = "HYUNDAI KONA 2020" - KONA_EV = "HYUNDAI KONA ELECTRIC 2019" - KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022" - KONA_EV_2ND_GEN = "HYUNDAI KONA ELECTRIC 2ND GEN" - KONA_HEV = "HYUNDAI KONA HYBRID 2020" - SANTA_FE = "HYUNDAI SANTA FE 2019" - SANTA_FE_2022 = "HYUNDAI SANTA FE 2022" - SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022" - SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022" - SONATA = "HYUNDAI SONATA 2020" - SONATA_LF = "HYUNDAI SONATA 2019" - STARIA_4TH_GEN = "HYUNDAI STARIA 4TH GEN" - TUCSON = "HYUNDAI TUCSON 2019" - PALISADE = "HYUNDAI PALISADE 2020" - VELOSTER = "HYUNDAI VELOSTER 2019" - SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" - IONIQ_5 = "HYUNDAI IONIQ 5 2022" - IONIQ_6 = "HYUNDAI IONIQ 6 2023" - TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN" - SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN" - CUSTIN_1ST_GEN = "HYUNDAI CUSTIN 1ST GEN" + # Static flags - # Kia - KIA_FORTE = "KIA FORTE E 2018 & GT 2021" - KIA_K5_2021 = "KIA K5 2021" - KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020" - KIA_K8_HEV_1ST_GEN = "KIA K8 HYBRID 1ST GEN" - KIA_NIRO_EV = "KIA NIRO EV 2020" - KIA_NIRO_EV_2ND_GEN = "KIA NIRO EV 2ND GEN" - KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" - KIA_NIRO_PHEV_2022 = "KIA NIRO PLUG-IN HYBRID 2022" - KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" - KIA_NIRO_HEV_2ND_GEN = "KIA NIRO HYBRID 2ND GEN" - KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN" - KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT" - KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" - KIA_OPTIMA_H_G4_FL = "KIA OPTIMA HYBRID 4TH GEN FACELIFT" - KIA_SELTOS = "KIA SELTOS 2021" - KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN" - KIA_SORENTO = "KIA SORENTO GT LINE 2018" - KIA_SORENTO_4TH_GEN = "KIA SORENTO 4TH GEN" - KIA_SORENTO_HEV_4TH_GEN = "KIA SORENTO HYBRID 4TH GEN" - KIA_STINGER = "KIA STINGER GT2 2018" - KIA_STINGER_2022 = "KIA STINGER 2022" - KIA_CEED = "KIA CEED INTRO ED 2019" - KIA_EV6 = "KIA EV6 2022" - KIA_CARNIVAL_4TH_GEN = "KIA CARNIVAL 4TH GEN" + # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. + # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py + MANDO_RADAR = 2 ** 12 + CANFD = 2 ** 13 - # Genesis - GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN" - GENESIS_G70 = "GENESIS G70 2018" - GENESIS_G70_2020 = "GENESIS G70 2020" - GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" - GENESIS_G80 = "GENESIS G80 2017" - GENESIS_G90 = "GENESIS G90 2017" - GENESIS_GV80 = "GENESIS GV80 2023" + # The radar does SCC on these cars when HDA I, rather than the camera + RADAR_SCC = 2 ** 14 + CAMERA_SCC = 2 ** 15 + CHECKSUM_CRC8 = 2 ** 16 + CHECKSUM_6B = 2 ** 17 + # these cars require a special panda safety mode due to missing counters and checksums in the messages + LEGACY = 2 ** 18 + + # these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc. + UNSUPPORTED_LONGITUDINAL = 2 ** 19 + + CANFD_NO_RADAR_DISABLE = 2 ** 20 + + CLUSTER_GEARS = 2 ** 21 + TCU_GEARS = 2 ** 22 + + MIN_STEER_32_MPH = 2 ** 23 + + CAN_LFA_BTN = 2 ** 24 class Footnote(Enum): CANFD = CarFootnote( @@ -151,164 +106,502 @@ class Footnote(Enum): @dataclass -class HyundaiCarInfo(CarInfo): +class HyundaiCarDocs(CarDocs): package: str = "Smart Cruise Control (SCC)" def init_make(self, CP: car.CarParams): - if CP.carFingerprint in CANFD_CAR: + if CP.flags & HyundaiFlags.CANFD: self.footnotes.insert(0, Footnote.CANFD) -CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { - CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.AZERA_HEV_6TH_GEN: [ - HyundaiCarInfo("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), - HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - ], - CAR.ELANTRA: [ - # TODO: 2017-18 could be Hyundai G - HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), - HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), - ], - CAR.ELANTRA_GT_I30: [ - HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), - HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), - ], - CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", - car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.HYUNDAI_GENESIS: [ - # TODO: check 2015 packages - HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), - HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), - ], - CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h])), # TODO: confirm 2020-21 harness - CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b])), - CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g])), - CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o])), - CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i])), # TODO: check packages - # TODO: this is the 2024 US MY, not yet released - CAR.KONA_EV_2ND_GEN: HyundaiCarInfo("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw", - car_parts=CarParts.common([CarHarness.hyundai_r])), - CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s", - car_parts=CarParts.common([CarHarness.hyundai_d])), - CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", - car_parts=CarParts.common([CarHarness.hyundai_l])), - CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), - CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), - CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", - car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.STARIA_4TH_GEN: HyundaiCarInfo("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e])), - CAR.TUCSON: [ - HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])), - HyundaiCarInfo("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])), - ], - CAR.PALISADE: [ - HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])), - HyundaiCarInfo("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - ], - CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e])), - CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.IONIQ_5: [ - HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])), - HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])), - HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), - ], - CAR.IONIQ_6: [ - HyundaiCarInfo("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])), - ], - CAR.TUCSON_4TH_GEN: [ - HyundaiCarInfo("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])), - HyundaiCarInfo("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), - HyundaiCarInfo("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), - ], - CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", car_parts=CarParts.common([CarHarness.hyundai_n])), - CAR.CUSTIN_1ST_GEN: HyundaiCarInfo("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), +@dataclass +class HyundaiPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict("hyundai_kia_generic", None)) + + def init(self): + if self.flags & HyundaiFlags.MANDO_RADAR: + self.dbc_dict = dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated') + + if self.flags & HyundaiFlags.MIN_STEER_32_MPH: + self.specs = self.specs.override(minSteerSpeed=32 * CV.MPH_TO_MS) + + +@dataclass +class HyundaiCanFDPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict("hyundai_canfd", None)) + + def init(self): + self.flags |= HyundaiFlags.CANFD + + +class CAR(Platforms): + # Hyundai + AZERA_6TH_GEN = HyundaiPlatformConfig( + "HYUNDAI AZERA 6TH GEN", + [HyundaiCarDocs("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=1600, wheelbase=2.885, steerRatio=14.5), + ) + AZERA_HEV_6TH_GEN = HyundaiPlatformConfig( + "HYUNDAI AZERA HYBRID 6TH GEN", + [ + HyundaiCarDocs("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), + ], + CarSpecs(mass=1675, wheelbase=2.885, steerRatio=14.5), + flags=HyundaiFlags.HYBRID, + ) + ELANTRA = HyundaiPlatformConfig( + "HYUNDAI ELANTRA 2017", + [ + # TODO: 2017-18 could be Hyundai G + HyundaiCarDocs("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), + HyundaiCarDocs("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), + ], + # steerRatio: 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535, stiffnessFactor settled on 1.0081302973865127 + CarSpecs(mass=1275, wheelbase=2.7, steerRatio=15.4, tireStiffnessFactor=0.385), + flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH, + ) + ELANTRA_GT_I30 = HyundaiPlatformConfig( + "HYUNDAI I30 N LINE 2019 & GT 2018 DCT", + [ + HyundaiCarDocs("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), + HyundaiCarDocs("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), + ], + ELANTRA.specs, + flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH, + ) + ELANTRA_2021 = HyundaiPlatformConfig( + "HYUNDAI ELANTRA 2021", + [HyundaiCarDocs("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=2800 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + ELANTRA_HEV_2021 = HyundaiPlatformConfig( + "HYUNDAI ELANTRA HYBRID 2021", + [HyundaiCarDocs("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", + car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=3017 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + HYUNDAI_GENESIS = HyundaiPlatformConfig( + "HYUNDAI GENESIS 2015-2016", + [ + # TODO: check 2015 packages + HyundaiCarDocs("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), + HyundaiCarDocs("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), + ], + CarSpecs(mass=2060, wheelbase=3.01, steerRatio=16.5, minSteerSpeed=60 * CV.KPH_TO_MS), + flags=HyundaiFlags.CHECKSUM_6B | HyundaiFlags.LEGACY, + ) + IONIQ = HyundaiPlatformConfig( + "HYUNDAI IONIQ HYBRID 2017-2019", + [HyundaiCarDocs("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH, + ) + IONIQ_HEV_2022 = HyundaiPlatformConfig( + "HYUNDAI IONIQ HYBRID 2020-2022", + [HyundaiCarDocs("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h]))], # TODO: confirm 2020-21 harness, + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, + ) + IONIQ_EV_LTD = HyundaiPlatformConfig( + "HYUNDAI IONIQ ELECTRIC LIMITED 2019", + [HyundaiCarDocs("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV | HyundaiFlags.LEGACY | HyundaiFlags.MIN_STEER_32_MPH, + ) + IONIQ_EV_2020 = HyundaiPlatformConfig( + "HYUNDAI IONIQ ELECTRIC 2020", + [HyundaiCarDocs("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.EV, + ) + IONIQ_PHEV_2019 = HyundaiPlatformConfig( + "HYUNDAI IONIQ PLUG-IN HYBRID 2019", + [HyundaiCarDocs("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH, + ) + IONIQ_PHEV = HyundaiPlatformConfig( + "HYUNDAI IONIQ PHEV 2020", + [HyundaiCarDocs("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID, + ) + KONA = HyundaiPlatformConfig( + "HYUNDAI KONA 2020", + [HyundaiCarDocs("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b]))], + CarSpecs(mass=1275, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.CLUSTER_GEARS, + ) + KONA_EV = HyundaiPlatformConfig( + "HYUNDAI KONA ELECTRIC 2019", + [HyundaiCarDocs("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g]))], + CarSpecs(mass=1685, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.EV, + ) + KONA_EV_2022 = HyundaiPlatformConfig( + "HYUNDAI KONA ELECTRIC 2022", + [HyundaiCarDocs("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o]))], + CarSpecs(mass=1743, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.CAMERA_SCC | HyundaiFlags.EV, + ) + KONA_EV_2ND_GEN = HyundaiCanFDPlatformConfig( + "HYUNDAI KONA ELECTRIC 2ND GEN", + [HyundaiCarDocs("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw", + car_parts=CarParts.common([CarHarness.hyundai_r]))], + CarSpecs(mass=1740, wheelbase=2.66, steerRatio=13.6, tireStiffnessFactor=0.385), + flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, + ) + KONA_HEV = HyundaiPlatformConfig( + "HYUNDAI KONA HYBRID 2020", + [HyundaiCarDocs("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i]))], # TODO: check packages, + CarSpecs(mass=1425, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID, + ) + SANTA_FE = HyundaiPlatformConfig( + "HYUNDAI SANTA FE 2019", + [HyundaiCarDocs("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s", + car_parts=CarParts.common([CarHarness.hyundai_d]))], + CarSpecs(mass=3982 * CV.LB_TO_KG, wheelbase=2.766, steerRatio=16.55, tireStiffnessFactor=0.82), + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + ) + SANTA_FE_2022 = HyundaiPlatformConfig( + "HYUNDAI SANTA FE 2022", + [HyundaiCarDocs("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", + car_parts=CarParts.common([CarHarness.hyundai_l]))], + SANTA_FE.specs, + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + SANTA_FE_HEV_2022 = HyundaiPlatformConfig( + "HYUNDAI SANTA FE HYBRID 2022", + [HyundaiCarDocs("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))], + SANTA_FE.specs, + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + SANTA_FE_PHEV_2022 = HyundaiPlatformConfig( + "HYUNDAI SANTA FE PlUG-IN HYBRID 2022", + [HyundaiCarDocs("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))], + SANTA_FE.specs, + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + SONATA = HyundaiPlatformConfig( + "HYUNDAI SONATA 2020", + [HyundaiCarDocs("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", + car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=1513, wheelbase=2.84, steerRatio=13.27 * 1.15, tireStiffnessFactor=0.65), # 15% higher at the center seems reasonable + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + ) + SONATA_LF = HyundaiPlatformConfig( + "HYUNDAI SONATA 2019", + [HyundaiCarDocs("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e]))], + CarSpecs(mass=1536, wheelbase=2.804, steerRatio=13.27 * 1.15), # 15% higher at the center seems reasonable + + flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, + ) + STARIA_4TH_GEN = HyundaiCanFDPlatformConfig( + "HYUNDAI STARIA 4TH GEN", + [HyundaiCarDocs("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=2205, wheelbase=3.273, steerRatio=11.94), # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf + ) + TUCSON = HyundaiPlatformConfig( + "HYUNDAI TUCSON 2019", + [ + HyundaiCarDocs("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])), + HyundaiCarDocs("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])), + ], + CarSpecs(mass=3520 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=16.1, tireStiffnessFactor=0.385), + flags=HyundaiFlags.TCU_GEARS, + ) + PALISADE = HyundaiPlatformConfig( + "HYUNDAI PALISADE 2020", + [ + HyundaiCarDocs("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])), + HyundaiCarDocs("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), + ], + CarSpecs(mass=1999, wheelbase=2.9, steerRatio=15.6 * 1.15, tireStiffnessFactor=0.63), + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + ) + VELOSTER = HyundaiPlatformConfig( + "HYUNDAI VELOSTER 2019", + [HyundaiCarDocs("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e]))], + CarSpecs(mass=2917 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75 * 1.15, tireStiffnessFactor=0.5), + flags=HyundaiFlags.LEGACY | HyundaiFlags.TCU_GEARS, + ) + SONATA_HYBRID = HyundaiPlatformConfig( + "HYUNDAI SONATA HYBRID 2021", + [HyundaiCarDocs("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], + SONATA.specs, + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + IONIQ_5 = HyundaiCanFDPlatformConfig( + "HYUNDAI IONIQ 5 2022", + [ + HyundaiCarDocs("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])), + HyundaiCarDocs("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])), + HyundaiCarDocs("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), + ], + CarSpecs(mass=1948, wheelbase=2.97, steerRatio=14.26, tireStiffnessFactor=0.65), + flags=HyundaiFlags.EV, + ) + IONIQ_6 = HyundaiCanFDPlatformConfig( + "HYUNDAI IONIQ 6 2023", + [HyundaiCarDocs("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))], + IONIQ_5.specs, + flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, + ) + TUCSON_4TH_GEN = HyundaiCanFDPlatformConfig( + "HYUNDAI TUCSON 4TH GEN", + [ + HyundaiCarDocs("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), + ], + CarSpecs(mass=1630, wheelbase=2.756, steerRatio=13.7, tireStiffnessFactor=0.385), + ) + SANTA_CRUZ_1ST_GEN = HyundaiCanFDPlatformConfig( + "HYUNDAI SANTA CRUZ 1ST GEN", + [HyundaiCarDocs("Hyundai Santa Cruz 2022-24", car_parts=CarParts.common([CarHarness.hyundai_n]))], + # weight from Limited trim - the only supported trim, steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf + CarSpecs(mass=1870, wheelbase=3, steerRatio=14.2), + ) + CUSTIN_1ST_GEN = HyundaiPlatformConfig( + "HYUNDAI CUSTIN 1ST GEN", + [HyundaiCarDocs("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=1690, wheelbase=3.055, steerRatio=17), # mass: from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0, steerRatio: from learner + flags=HyundaiFlags.CHECKSUM_CRC8, + ) # Kia - CAR.KIA_FORTE: [ - HyundaiCarInfo("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])), - HyundaiCarInfo("Kia Forte 2023", car_parts=CarParts.common([CarHarness.hyundai_e])), - ], - CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_K8_HEV_1ST_GEN: HyundaiCarInfo("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), - CAR.KIA_NIRO_EV: [ - HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), - HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])), - HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])), - HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), - ], - CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_NIRO_PHEV: [ - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_d])), - ], - CAR.KIA_NIRO_PHEV_2022: [ - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2021", "All", car_parts=CarParts.common([CarHarness.hyundai_d])), - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), - ], - CAR.KIA_NIRO_HEV_2021: [ - HyundaiCarInfo("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), - HyundaiCarInfo("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), - ], - CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", - car_parts=CarParts.common([CarHarness.hyundai_b])), # TODO: may support 2016, 2018 - CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g])), + KIA_FORTE = HyundaiPlatformConfig( + "KIA FORTE E 2018 & GT 2021", + [ + HyundaiCarDocs("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])), + HyundaiCarDocs("Kia Forte 2023", car_parts=CarParts.common([CarHarness.hyundai_e])), + ], + CarSpecs(mass=2878 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5) + ) + KIA_K5_2021 = HyundaiPlatformConfig( + "KIA K5 2021", + [HyundaiCarDocs("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=3381 * CV.LB_TO_KG, wheelbase=2.85, steerRatio=13.27, tireStiffnessFactor=0.5), # 2021 Kia K5 Steering Ratio (all trims) + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + KIA_K5_HEV_2020 = HyundaiPlatformConfig( + "KIA K5 HYBRID 2020", + [HyundaiCarDocs("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a]))], + KIA_K5_2021.specs, + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + KIA_K8_HEV_1ST_GEN = HyundaiCanFDPlatformConfig( + "KIA K8 HYBRID 1ST GEN", + [HyundaiCarDocs("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q]))], + # mass: https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid, steerRatio: guesstimate from K5 platform + CarSpecs(mass=1630, wheelbase=2.895, steerRatio=13.27) + ) + KIA_NIRO_EV = HyundaiPlatformConfig( + "KIA NIRO EV 2020", + [ + HyundaiCarDocs("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), + HyundaiCarDocs("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])), + HyundaiCarDocs("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), + ], + CarSpecs(mass=3543 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=13.6, tireStiffnessFactor=0.385), # average of all the cars + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV, + ) + KIA_NIRO_EV_2ND_GEN = HyundaiCanFDPlatformConfig( + "KIA NIRO EV 2ND GEN", + [HyundaiCarDocs("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.EV, + ) + KIA_NIRO_PHEV = HyundaiPlatformConfig( + "KIA NIRO HYBRID 2019", + [ + HyundaiCarDocs("Kia Niro Hybrid 2018", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_d])), + ], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.HYBRID | HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.MIN_STEER_32_MPH, + ) + KIA_NIRO_PHEV_2022 = HyundaiPlatformConfig( + "KIA NIRO PLUG-IN HYBRID 2022", + [ + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), + ], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.HYBRID | HyundaiFlags.MANDO_RADAR, + ) + KIA_NIRO_HEV_2021 = HyundaiPlatformConfig( + "KIA NIRO HYBRID 2021", + [ + HyundaiCarDocs("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), + HyundaiCarDocs("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), + ], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.HYBRID, + ) + KIA_NIRO_HEV_2ND_GEN = HyundaiCanFDPlatformConfig( + "KIA NIRO HYBRID 2ND GEN", + [HyundaiCarDocs("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a]))], + KIA_NIRO_EV.specs, + ) + KIA_OPTIMA_G4 = HyundaiPlatformConfig( + "KIA OPTIMA 4TH GEN", + [HyundaiCarDocs("Kia Optima 2017", "Advanced Smart Cruise Control", + car_parts=CarParts.common([CarHarness.hyundai_b]))], # TODO: may support 2016, 2018 + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.LEGACY | HyundaiFlags.TCU_GEARS | HyundaiFlags.MIN_STEER_32_MPH, + ) + KIA_OPTIMA_G4_FL = HyundaiPlatformConfig( + "KIA OPTIMA 4TH GEN FACELIFT", + [HyundaiCarDocs("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g]))], + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, + ) # TODO: may support adjacent years. may have a non-zero minimum steering speed - CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.KIA_OPTIMA_H_G4_FL: HyundaiCarInfo("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_SPORTAGE_5TH_GEN: [ - HyundaiCarInfo("Kia Sportage 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), - HyundaiCarInfo("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), - ], - CAR.KIA_SORENTO: [ - HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", - car_parts=CarParts.common([CarHarness.hyundai_e])), - HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])), - ], - CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.KIA_SORENTO_HEV_4TH_GEN: [ - HyundaiCarInfo("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - ], - CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", - car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e])), - CAR.KIA_EV6: [ - HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])), - HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])), - HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])) - ], - CAR.KIA_CARNIVAL_4TH_GEN: [ - HyundaiCarInfo("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])), - HyundaiCarInfo("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k])) - ], + KIA_OPTIMA_H = HyundaiPlatformConfig( + "KIA OPTIMA HYBRID 2017 & SPORTS 2019", + [HyundaiCarDocs("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, + ) + KIA_OPTIMA_H_G4_FL = HyundaiPlatformConfig( + "KIA OPTIMA HYBRID 4TH GEN FACELIFT", + [HyundaiCarDocs("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.HYBRID | HyundaiFlags.UNSUPPORTED_LONGITUDINAL, + ) + KIA_SELTOS = HyundaiPlatformConfig( + "KIA SELTOS 2021", + [HyundaiCarDocs("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=1337, wheelbase=2.63, steerRatio=14.56), + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + KIA_SPORTAGE_5TH_GEN = HyundaiCanFDPlatformConfig( + "KIA SPORTAGE 5TH GEN", + [ + HyundaiCarDocs("Kia Sportage 2023-24", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), + ], + # weight from SX and above trims, average of FWD and AWD version, steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications + CarSpecs(mass=1725, wheelbase=2.756, steerRatio=13.6), + ) + KIA_SORENTO = HyundaiPlatformConfig( + "KIA SORENTO GT LINE 2018", + [ + HyundaiCarDocs("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", + car_parts=CarParts.common([CarHarness.hyundai_e])), + HyundaiCarDocs("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])), + ], + CarSpecs(mass=1985, wheelbase=2.78, steerRatio=14.4 * 1.1), # 10% higher at the center seems reasonable + flags=HyundaiFlags.CHECKSUM_6B | HyundaiFlags.UNSUPPORTED_LONGITUDINAL, + ) + KIA_SORENTO_4TH_GEN = HyundaiCanFDPlatformConfig( + "KIA SORENTO 4TH GEN", + [HyundaiCarDocs("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=3957 * CV.LB_TO_KG, wheelbase=2.81, steerRatio=13.5), # average of the platforms + flags=HyundaiFlags.RADAR_SCC, + ) + KIA_SORENTO_HEV_4TH_GEN = HyundaiCanFDPlatformConfig( + "KIA SORENTO HYBRID 4TH GEN", + [ + HyundaiCarDocs("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), + ], + CarSpecs(mass=4395 * CV.LB_TO_KG, wheelbase=2.81, steerRatio=13.5), # average of the platforms + flags=HyundaiFlags.RADAR_SCC, + ) + KIA_STINGER = HyundaiPlatformConfig( + "KIA STINGER GT2 2018", + [HyundaiCarDocs("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", + car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1825, wheelbase=2.78, steerRatio=14.4 * 1.15) # 15% higher at the center seems reasonable + ) + KIA_STINGER_2022 = HyundaiPlatformConfig( + "KIA STINGER 2022", + [HyundaiCarDocs("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + KIA_STINGER.specs, + ) + KIA_CEED = HyundaiPlatformConfig( + "KIA CEED INTRO ED 2019", + [HyundaiCarDocs("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e]))], + CarSpecs(mass=1450, wheelbase=2.65, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.LEGACY, + ) + KIA_EV6 = HyundaiCanFDPlatformConfig( + "KIA EV6 2022", + [ + HyundaiCarDocs("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])), + HyundaiCarDocs("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])), + HyundaiCarDocs("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])) + ], + CarSpecs(mass=2055, wheelbase=2.9, steerRatio=16, tireStiffnessFactor=0.65), + flags=HyundaiFlags.EV, + ) + KIA_CARNIVAL_4TH_GEN = HyundaiCanFDPlatformConfig( + "KIA CARNIVAL 4TH GEN", + [ + HyundaiCarDocs("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k])) + ], + CarSpecs(mass=2087, wheelbase=3.09, steerRatio=14.23), + flags=HyundaiFlags.RADAR_SCC, + ) # Genesis - CAR.GENESIS_GV60_EV_1ST_GEN: [ - HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - ], - CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), - CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), - CAR.GENESIS_GV70_1ST_GEN: [ - HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), - HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), - ], - CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), -} + GENESIS_GV60_EV_1ST_GEN = HyundaiCanFDPlatformConfig( + "GENESIS GV60 ELECTRIC 1ST GEN", + [ + HyundaiCarDocs("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), + ], + CarSpecs(mass=2205, wheelbase=2.9, steerRatio=12.6), # steerRatio: https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1. + flags=HyundaiFlags.EV, + ) + GENESIS_G70 = HyundaiPlatformConfig( + "GENESIS G70 2018", + [HyundaiCarDocs("Genesis G70 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_f]))], + CarSpecs(mass=1640, wheelbase=2.84, steerRatio=13.56), + flags=HyundaiFlags.LEGACY, + ) + GENESIS_G70_2020 = HyundaiPlatformConfig( + "GENESIS G70 2020", + [HyundaiCarDocs("Genesis G70 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_f]))], + CarSpecs(mass=3673 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=12.9), + flags=HyundaiFlags.MANDO_RADAR, + ) + GENESIS_GV70_1ST_GEN = HyundaiCanFDPlatformConfig( + "GENESIS GV70 1ST GEN", + [ + HyundaiCarDocs("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), + HyundaiCarDocs("Genesis GV70 (3.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), + ], + CarSpecs(mass=1950, wheelbase=2.87, steerRatio=14.6), + flags=HyundaiFlags.RADAR_SCC, + ) + GENESIS_G80 = HyundaiPlatformConfig( + "GENESIS G80 2017", + [HyundaiCarDocs("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=2060, wheelbase=3.01, steerRatio=16.5), + flags=HyundaiFlags.LEGACY, + ) + GENESIS_G90 = HyundaiPlatformConfig( + "GENESIS G90 2017", + [HyundaiCarDocs("Genesis G90 2017-20", "All", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=2200, wheelbase=3.15, steerRatio=12.069), + ) + GENESIS_GV80 = HyundaiCanFDPlatformConfig( + "GENESIS GV80 2023", + [HyundaiCarDocs("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m]))], + CarSpecs(mass=2258, wheelbase=2.95, steerRatio=14.14), + flags=HyundaiFlags.RADAR_SCC, + ) + class Buttons: NONE = 0 @@ -318,7 +611,7 @@ class Buttons: CANCEL = 4 # on newer models, this is a pause/resume button -def get_platform_codes(fw_versions: List[bytes]) -> Set[Tuple[bytes, Optional[bytes]]]: +def get_platform_codes(fw_versions: list[bytes]) -> set[tuple[bytes, bytes | None]]: # Returns unique, platform-specific identification codes for a set of versions codes = set() # (code-Optional[part], date) for fw in fw_versions: @@ -337,12 +630,12 @@ def get_platform_codes(fw_versions: List[bytes]) -> Set[Tuple[bytes, Optional[by return codes -def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> Set[str]: +def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> set[str]: # Non-electric CAN FD platforms often do not have platform code specifiers needed # to distinguish between hybrid and ICE. All EVs so far are either exclusively # electric or specify electric in the platform code. fuzzy_platform_blacklist = {str(c) for c in (CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST)} - candidates: Set[str] = set() + candidates: set[str] = set() for candidate, fws in offline_fw_versions.items(): # Keep track of ECUs which pass all checks (platform codes, within date range) @@ -410,7 +703,9 @@ DATE_FW_PATTERN = re.compile(b'(?<=[ -])([0-9]{6}$)') PART_NUMBER_FW_PATTERN = re.compile(b'(?<=[0-9][.,][0-9]{2} )([0-9]{5}[-/]?[A-Z][A-Z0-9]{3}[0-9])') # We've seen both ICE and hybrid for these platforms, and they have hybrid descriptors (e.g. MQ4 vs MQ4H) -CANFD_FUZZY_WHITELIST = {CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN} +CANFD_FUZZY_WHITELIST = {CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_K8_HEV_1ST_GEN, + # TODO: the hybrid variant is not out yet + CAR.KIA_CARNIVAL_4TH_GEN} # List of ECUs expected to have platform codes, camera and radar should exist on all cars # TODO: use abs, it has the platform code and part number on many platforms @@ -419,7 +714,8 @@ PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps] # TODO: there are date codes in the ABS firmware versions in hex DATE_FW_ECUS = [Ecu.fwdCamera] -ALL_HYUNDAI_ECUS = [Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.engine, Ecu.parkingAdas, Ecu.transmission, Ecu.adas, Ecu.hvac, Ecu.cornerRadar] +ALL_HYUNDAI_ECUS = [Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.engine, Ecu.parkingAdas, + Ecu.transmission, Ecu.adas, Ecu.hvac, Ecu.cornerRadar, Ecu.combinationMeter] FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -441,7 +737,7 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [HYUNDAI_VERSION_REQUEST_LONG], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac], + whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac, Ecu.eps], bus=0, auxiliary=True, ), @@ -511,128 +807,54 @@ FW_QUERY_CONFIG = FwQueryConfig( obd_multiplexing=False, ), ], + # We lose these ECUs without the comma power on these cars. + # Note that we still attempt to match with them when they are present + non_essential_ecus={ + Ecu.transmission: [CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.PALISADE, CAR.SONATA], + Ecu.engine: [CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.PALISADE, CAR.SONATA], + Ecu.abs: [CAR.PALISADE, CAR.SONATA], + }, extra_ecus=[ - (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms - (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) - (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly + (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms + (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) + (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly (Ecu.cornerRadar, 0x7b7, None), + (Ecu.combinationMeter, 0x7c6, None), # CAN FD Instrument cluster ], # Custom fuzzy fingerprinting function using platform codes, part numbers + FW dates: match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, ) - CHECKSUM = { - "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, - CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, - CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN], - "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], + "crc8": CAR.with_flags(HyundaiFlags.CHECKSUM_CRC8), + "6B": CAR.with_flags(HyundaiFlags.CHECKSUM_6B), } CAN_GEARS = { # which message has the gear. hybrid and EV use ELECT_GEAR - "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, - "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, + "use_cluster_gears": CAR.with_flags(HyundaiFlags.CLUSTER_GEARS), + "use_tcu_gears": CAR.with_flags(HyundaiFlags.TCU_GEARS), } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, - CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, - CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.KIA_K8_HEV_1ST_GEN, - CAR.STARIA_4TH_GEN} - -# The radar does SCC on these cars when HDA I, rather than the camera -CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN} +CANFD_CAR = CAR.with_flags(HyundaiFlags.CANFD) +CANFD_RADAR_SCC_CAR = CAR.with_flags(HyundaiFlags.RADAR_SCC) # These CAN FD cars do not accept communication control to disable the ADAS ECU, # responds with 0x7F2822 - 'conditions not correct' -CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN} +CANFD_UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.CANFD_NO_RADAR_DISABLE) # The camera does SCC on these cars, rather than the radar -CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } +CAMERA_SCC_CAR = CAR.with_flags(HyundaiFlags.CAMERA_SCC) -# these cars use a different gas signal -HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, - CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KIA_K5_HEV_2020, - CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.AZERA_HEV_6TH_GEN, CAR.KIA_NIRO_PHEV_2022} +HYBRID_CAR = CAR.with_flags(HyundaiFlags.HYBRID) -EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022, - CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KONA_EV_2ND_GEN} +EV_CAR = CAR.with_flags(HyundaiFlags.EV) -# these cars require a special panda safety mode due to missing counters and checksums in the messages -LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4, - CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022, - CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} +LEGACY_SAFETY_MODE_CAR = CAR.with_flags(HyundaiFlags.LEGACY) -# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc. -UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL, - CAR.KIA_OPTIMA_H_G4_FL} +UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL) -# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. -# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py -DBC = { - CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None), - CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None), - CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_K5_HEV_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_H_G4_FL: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format - CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None), - CAR.KONA: dbc_dict('hyundai_kia_generic', None), - CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None), - CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format - CAR.TUCSON: dbc_dict('hyundai_kia_generic', None), - CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_EV6: dbc_dict('hyundai_canfd', None), - CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.IONIQ_5: dbc_dict('hyundai_canfd', None), - CAR.IONIQ_6: dbc_dict('hyundai_canfd', None), - CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None), - CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None), - CAR.KIA_CARNIVAL_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_SORENTO_HEV_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.KONA_EV_2ND_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_K8_HEV_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.CUSTIN_1ST_GEN: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_NIRO_PHEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.STARIA_4TH_GEN: dbc_dict('hyundai_canfd', None), -} +DBC = CAR.create_dbc_map() + +if __name__ == "__main__": + CAR.print_debug(HyundaiFlags) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 0697bee..dfb0b38 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -7,6 +7,7 @@ from difflib import SequenceMatcher from enum import StrEnum from json import load from typing import Any, Callable, Dict, List, NamedTuple, Optional, Tuple, Union +from collections.abc import Callable from cereal import car from openpilot.common.basedir import BASEDIR @@ -16,14 +17,11 @@ from openpilot.common.numpy_fast import clip from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction +from openpilot.selfdrive.car.values import PLATFORMS +from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS, V_CRUISE_MAX, get_friction from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import FrogPilotFunctions - -params_memory = Params("/dev/shm/params") - ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName @@ -209,20 +207,37 @@ class CarInterfaceBase(ABC): self.CC = CarController(self.cp.dbc_name, CP, self.VM) # FrogPilot variables - params = Params() + self.params = Params() + self.params_memory = Params("/dev/shm/params") - self.has_lateral_torque_nn = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware) and params.get_bool("NNFF") and params.get_bool("LateralTune") - self.use_lateral_jerk = params.get_bool("UseLateralJerk") and params.get_bool("LateralTune") + lateral_tune = self.params.get_bool("LateralTune") + nnff_supported = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware) + use_comma_nnff = self.check_comma_nn_ff_support(CP.carFingerprint) + self.use_nnff = not use_comma_nnff and nnff_supported and lateral_tune and self.params.get_bool("NNFF") + self.use_nnff_lite = not use_comma_nnff and not nnff_supported and lateral_tune and self.params.get_bool("NNFFLite") self.belowSteerSpeed_shown = False self.disable_belowSteerSpeed = False - - self.resumeRequired_shown = False self.disable_resumeRequired = False + self.prev_distance_button = False + self.resumeRequired_shown = False + self.traffic_mode_changed = False + + self.gap_counter = 0 def get_ff_nn(self, x): return self.lat_torque_nn_model.evaluate(x) + def check_comma_nn_ff_support(self, car): + try: + with open("../car/torque_data/neural_ff_weights.json", "r") as file: + data = json.load(file) + return car in data + + except FileNotFoundError: + print("Failed to open neural_ff_weights file.") + return False + def initialize_lat_torque_nn(self, car, eps_firmware): self.lat_torque_nn_model, _ = get_nn_model(car, eps_firmware) return (self.lat_torque_nn_model is not None) @@ -239,12 +254,23 @@ class CarInterfaceBase(ABC): """ Parameters essential to controlling the car may be incomplete or wrong without FW versions or fingerprints. """ - return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False) + return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False, False) @classmethod - def get_params(cls, params, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool): + def get_params(cls, params, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], disable_openpilot_long: bool, experimental_long: bool, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) - ret = cls._get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs) + + platform = PLATFORMS[candidate] + ret.mass = platform.config.specs.mass + ret.wheelbase = platform.config.specs.wheelbase + ret.steerRatio = platform.config.specs.steerRatio + ret.centerToFront = ret.wheelbase * platform.config.specs.centerToFrontRatio + ret.minEnableSpeed = platform.config.specs.minEnableSpeed + ret.minSteerSpeed = platform.config.specs.minSteerSpeed + ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor + ret.flags |= int(platform.config.flags) + + ret = cls._get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs) # Enable torque controller for all cars that do not use angle based steering if ret.steerControlType != car.CarParams.SteerControlType.angle and params.get_bool("LateralTune") and params.get_bool("NNFF"): @@ -252,8 +278,7 @@ class CarInterfaceBase(ABC): eps_firmware = str(next((fw.fwVersion for fw in car_fw if fw.ecu == "eps"), "")) model, similarity_score = get_nn_model_path(candidate, eps_firmware) if model is not None: - params_memory.put_bool("NNFFModelFuzzyMatch", similarity_score < 0.99) - params_memory.put("NNFFModelName", candidate) + params.put("NNFFModelName", candidate) # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload if not ret.notCar: @@ -267,8 +292,8 @@ class CarInterfaceBase(ABC): @staticmethod @abstractmethod - def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], - car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool): + def _get_params(ret: car.CarParams, candidate, fingerprint: dict[int, dict[int, int]], + car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): raise NotImplementedError @staticmethod @@ -348,7 +373,7 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: List[bytes], frogpilot_variables) -> car.CarState: + def update(self, c: car.CarControl, can_strings: list[bytes], frogpilot_variables) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: @@ -374,6 +399,10 @@ class CarInterfaceBase(ABC): if ret.cruiseState.speedCluster == 0: ret.cruiseState.speedCluster = ret.cruiseState.speed + distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed") + self.params_memory.put_bool("DistanceLongPressed", self.frogpilot_distance_functions(distance_button, self.prev_distance_button, frogpilot_variables)) + self.prev_distance_button = distance_button + # copy back for next iteration reader = ret.as_reader() if self.CS is not None: @@ -382,10 +411,10 @@ class CarInterfaceBase(ABC): return reader @abstractmethod - def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]: + def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[bytes]]: pass - def create_common_events(self, cs_out, frogpilot_variables, extra_gears=None, pcm_enable=True, allow_enable=True, + def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): events = Events() @@ -458,6 +487,28 @@ class CarInterfaceBase(ABC): return events + def frogpilot_distance_functions(self, distance_button, prev_distance_button, frogpilot_variables): + if distance_button: + self.gap_counter += 1 + elif not prev_distance_button: + self.gap_counter = 0 + + if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance or self.traffic_mode_changed: + if frogpilot_variables.conditional_experimental_mode: + conditional_status = self.params_memory.get_int("CEStatus") + override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2 + self.params_memory.put_int("CEStatus", override_value) + else: + experimental_mode = self.params.get_bool("ExperimentalMode") + self.params.put_bool("ExperimentalMode", not experimental_mode) + self.traffic_mode_changed = False + + if self.gap_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: + traffic_mode = self.params_memory.get_bool("TrafficModeActive") + self.params_memory.put_bool("TrafficModeActive", not traffic_mode) + self.traffic_mode_changed = frogpilot_variables.experimental_mode_via_distance + + return self.gap_counter >= CRUISE_LONG_PRESS class RadarInterfaceBase(ABC): def __init__(self, CP): @@ -466,7 +517,6 @@ class RadarInterfaceBase(ABC): self.delay = 0 self.radar_ts = CP.radarTimeStep self.frame = 0 - self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ def update(self, can_strings): self.frame += 1 @@ -499,15 +549,17 @@ class CarStateBase(ABC): self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) # FrogPilot variables - self.fpf = FrogPilotFunctions() + self.params_memory = Params("/dev/shm/params") - self.distance_button = False - self.distance_previously_pressed = False - self.lkas_previously_pressed = False + self.cruise_decreased = False + self.cruise_decreased_previously = False + self.cruise_increased = False + self.cruise_increased_previously = False + self.lkas_enabled = False + self.lkas_previously_enabled = False - self.distance_pressed_counter = 0 - self.personality_profile = self.fpf.current_personality - self.previous_personality_profile = self.personality_profile + self.prev_distance_button = 0 + self.distance_button = 0 def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed @@ -564,11 +616,11 @@ class CarStateBase(ABC): return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0) @staticmethod - def parse_gear_shifter(gear: Optional[str]) -> car.CarState.GearShifter: + def parse_gear_shifter(gear: str | None) -> car.CarState.GearShifter: if gear is None: return GearShifter.unknown - d: Dict[str, car.CarState.GearShifter] = { + d: dict[str, car.CarState.GearShifter] = { 'P': GearShifter.park, 'PARK': GearShifter.park, 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, @@ -598,6 +650,15 @@ class CarStateBase(ABC): return None +SendCan = tuple[int, int, bytes, int] + + +class CarControllerBase(ABC): + @abstractmethod + def update(self, CC, CS, now_nanos) -> tuple[car.CarControl.Actuators, list[SendCan]]: + pass + + INTERFACE_ATTR_FILE = { "FINGERPRINTS": "fingerprints", "FW_VERSIONS": "fingerprints", @@ -605,7 +666,7 @@ INTERFACE_ATTR_FILE = { # interface-specific helpers -def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str | StrEnum, Any]: +def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> dict[str | StrEnum, Any]: # read all the folders in selfdrive/car and return a dict where: # - keys are all the car models or brand names # - values are attr values from all car folders @@ -638,7 +699,7 @@ class NanoFFModel: self.load_weights(platform) def load_weights(self, platform: str): - with open(self.weights_loc, 'r') as fob: + with open(self.weights_loc) as fob: self.weights = {k: np.array(v) for k, v in json.load(fob)[platform].items()} def relu(self, x: np.ndarray): @@ -653,7 +714,7 @@ class NanoFFModel: x = np.dot(x, self.weights['w_4']) + self.weights['b_4'] return x - def predict(self, x: List[float], do_sample: bool = False): + def predict(self, x: list[float], do_sample: bool = False): x = self.forward(np.array(x)) if do_sample: pred = np.random.laplace(x[0], np.exp(x[1]) / self.weights['temperature']) diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 678fe9e..8fdc747 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -12,7 +12,7 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a class IsoTpParallelQuery: def __init__(self, sendcan: messaging.PubSocket, logcan: messaging.SubSocket, bus: int, addrs: list[int] | list[AddrType], request: list[bytes], response: list[bytes], response_offset: int = 0x8, - functional_addrs: list[int] | None = None, debug: bool = False, response_pending_timeout: float = 10) -> None: + functional_addrs: list[int] = None, debug: bool = False, response_pending_timeout: float = 10) -> None: self.sendcan = sendcan self.logcan = logcan self.bus = bus diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 4952989..16206f6 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,13 +1,14 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.mazda import mazdacan from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 @@ -35,13 +36,13 @@ class CarController: if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): # Cancel Stock ACC if it's enabled while OP is disengaged # Send at a rate of 10hz until we sync with stock ACC state - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL)) else: self.brake_counter = 0 if CC.cruiseControl.resume and self.frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button when planner wants car to move - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME)) self.apply_steer_last = apply_steer @@ -54,7 +55,7 @@ class CarController: can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) # send steering command - can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint, + can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, self.frame, apply_steer, CS.cam_lkas)) new_actuators = CC.actuators.copy() diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 6b930e6..9b236b2 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1 +from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags class CarState(CarStateBase): def __init__(self, CP): @@ -18,9 +18,16 @@ class CarState(CarStateBase): self.lkas_allowed_speed = False self.lkas_disabled = False + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() + + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] + ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], cp.vl["WHEEL_SPEEDS"]["FR"], @@ -103,6 +110,9 @@ class CarState(CarStateBase): self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = not self.lkas_disabled + return ret @staticmethod @@ -116,7 +126,7 @@ class CarState(CarStateBase): ("WHEEL_SPEEDS", 100), ] - if CP.carFingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: messages += [ ("ENGINE_DATA", 100), ("CRZ_CTRL", 50), @@ -136,7 +146,7 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): messages = [] - if CP.carFingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: messages += [ # sig_address, frequency ("CAM_LANEINFO", 2), diff --git a/selfdrive/car/mazda/fingerprints.py b/selfdrive/car/mazda/fingerprints.py index 292f407..8143ad7 100644 --- a/selfdrive/car/mazda/fingerprints.py +++ b/selfdrive/car/mazda/fingerprints.py @@ -10,6 +10,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'PEW5-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PW67-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2H-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -31,6 +32,7 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'PG69-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PW66-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXFG-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 659234c..3ef577b 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,17 +1,18 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True @@ -20,27 +21,9 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.8 - ret.tireStiffnessFactor = 0.70 # not optimized yet CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate in (CAR.CX5, CAR.CX5_2022): - ret.mass = 3655 * CV.LB_TO_KG - ret.wheelbase = 2.7 - ret.steerRatio = 15.5 - elif candidate in (CAR.CX9, CAR.CX9_2021): - ret.mass = 4217 * CV.LB_TO_KG - ret.wheelbase = 3.1 - ret.steerRatio = 17.6 - elif candidate == CAR.MAZDA3: - ret.mass = 2875 * CV.LB_TO_KG - ret.wheelbase = 2.7 - ret.steerRatio = 14.0 - elif candidate == CAR.MAZDA6: - ret.mass = 3443 * CV.LB_TO_KG - ret.wheelbase = 2.83 - ret.steerRatio = 15.5 - if candidate not in (CAR.CX5_2022, ): ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS @@ -52,8 +35,14 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + # TODO: add button types for inc and dec + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + # events - events = self.create_common_events(ret, frogpilot_variables) + events = self.create_common_events(ret) if self.CS.lkas_disabled: events.add(EventName.lkasDisabled) diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index e350c55..74f6af0 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,7 +1,7 @@ -from openpilot.selfdrive.car.mazda.values import GEN1, Buttons +from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags -def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): +def create_steering_control(packer, CP, frame, apply_steer, lkas): tmp = apply_steer + 2048 @@ -45,7 +45,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): csum = csum % 256 values = {} - if car_fingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: values = { "LKAS_REQUEST": apply_steer, "CTR": ctr, @@ -88,12 +88,12 @@ def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool) return packer.make_can_msg("CAM_LANEINFO", 0, values) -def create_button_cmd(packer, car_fingerprint, counter, button): +def create_button_cmd(packer, CP, counter, button): can = int(button == Buttons.CANCEL) res = int(button == Buttons.RESUME) - if car_fingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: values = { "CAN_OFF": can, "CAN_OFF_INV": (can + 1) % 2, diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index b43ab3d..d10b47e 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,10 +1,10 @@ from dataclasses import dataclass, field -from enum import StrEnum -from typing import Dict, List, Union +from enum import IntFlag from cereal import car -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -26,29 +26,60 @@ class CarControllerParams: pass -class CAR(StrEnum): - CX5 = "MAZDA CX-5" - CX9 = "MAZDA CX-9" - MAZDA3 = "MAZDA 3" - MAZDA6 = "MAZDA 6" - CX9_2021 = "MAZDA CX-9 2021" - CX5_2022 = "MAZDA CX-5 2022" - - @dataclass -class MazdaCarInfo(CarInfo): +class MazdaCarDocs(CarDocs): package: str = "All" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.mazda])) -CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { - CAR.CX5: MazdaCarInfo("Mazda CX-5 2017-21"), - CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"), - CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), - CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), - CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"), - CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"), -} +@dataclass(frozen=True, kw_only=True) +class MazdaCarSpecs(CarSpecs): + tireStiffnessFactor: float = 0.7 # not optimized yet + + +class MazdaFlags(IntFlag): + # Static flags + # Gen 1 hardware: same CAN messages and same camera + GEN1 = 1 + + +@dataclass +class MazdaPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('mazda_2017', None)) + flags: int = MazdaFlags.GEN1 + + +class CAR(Platforms): + CX5 = MazdaPlatformConfig( + "MAZDA CX-5", + [MazdaCarDocs("Mazda CX-5 2017-21")], + MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5) + ) + CX9 = MazdaPlatformConfig( + "MAZDA CX-9", + [MazdaCarDocs("Mazda CX-9 2016-20")], + MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6) + ) + MAZDA3 = MazdaPlatformConfig( + "MAZDA 3", + [MazdaCarDocs("Mazda 3 2017-18")], + MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0) + ) + MAZDA6 = MazdaPlatformConfig( + "MAZDA 6", + [MazdaCarDocs("Mazda 6 2017-20")], + MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5) + ) + CX9_2021 = MazdaPlatformConfig( + "MAZDA CX-9 2021", + [MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")], + CX9.specs + ) + CX5_2022 = MazdaPlatformConfig( + "MAZDA CX-5 2022", + [MazdaCarDocs("Mazda CX-5 2022-24")], + CX5.specs, + ) class LKAS_LIMITS: @@ -76,15 +107,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.CX5: dbc_dict('mazda_2017', None), - CAR.CX9: dbc_dict('mazda_2017', None), - CAR.MAZDA3: dbc_dict('mazda_2017', None), - CAR.MAZDA6: dbc_dict('mazda_2017', None), - CAR.CX9_2021: dbc_dict('mazda_2017', None), - CAR.CX5_2022: dbc_dict('mazda_2017', None), -} - -# Gen 1 hardware: same CAN messages and same camera -GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 90aa3be..7368f1a 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -12,7 +12,7 @@ class CarInterface(CarInterfaceBase): self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "mock" ret.mass = 1700. ret.wheelbase = 2.70 diff --git a/selfdrive/car/mock/values.py b/selfdrive/car/mock/values.py index c6c9657..214c680 100644 --- a/selfdrive/car/mock/values.py +++ b/selfdrive/car/mock/values.py @@ -1,13 +1,10 @@ -from enum import StrEnum -from typing import Dict, List, Optional, Union - -from openpilot.selfdrive.car.docs_definitions import CarInfo +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms -class CAR(StrEnum): - MOCK = 'mock' - - -CAR_INFO: Dict[str, Optional[Union[CarInfo, List[CarInfo]]]] = { - CAR.MOCK: None, -} +class CAR(Platforms): + MOCK = PlatformConfig( + 'mock', + [], + CarSpecs(mass=1700, wheelbase=2.7, steerRatio=13), + {} + ) diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 96ec5c6..0d4e857 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,13 +1,14 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.nissan import nissancan from openpilot.selfdrive.car.nissan.values import CAR, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.car_fingerprint = CP.carFingerprint diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 1d0052b..13f7fe3 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -20,9 +20,15 @@ class CarState(CarStateBase): self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, cp, cp_adas, cp_cam, frogpilot_variables): ret = car.CarState.new_message() + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["CRUISE_THROTTLE"]["FOLLOW_DISTANCE_BUTTON"] + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): @@ -101,6 +107,7 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint == CAR.ALTIMA: self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) else: diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index cfc10aa..cd89d29 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,14 +1,17 @@ -from cereal import car +from cereal import car, custom from panda import Panda -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.nissan.values import CAR +ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type + class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False @@ -16,25 +19,13 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.1 - ret.steerRatio = 17 ret.steerControlType = car.CarParams.SteerControlType.angle ret.radarUnavailable = True - if candidate in (CAR.ROGUE, CAR.XTRAIL): - ret.mass = 1610 - ret.wheelbase = 2.705 - ret.centerToFront = ret.wheelbase * 0.44 - elif candidate in (CAR.LEAF, CAR.LEAF_IC): - ret.mass = 1610 - ret.wheelbase = 2.705 - ret.centerToFront = ret.wheelbase * 0.44 - elif candidate == CAR.ALTIMA: + if candidate == CAR.ALTIMA: # Altima has EPS on C-CAN unlike the others that have it on V-CAN ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS - ret.mass = 1492 - ret.wheelbase = 2.824 - ret.centerToFront = ret.wheelbase * 0.44 return ret @@ -42,12 +33,12 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_variables) - buttonEvents = [] - be = car.CarState.ButtonEvent.new_message() - be.type = car.CarState.ButtonEvent.Type.accelCruise - buttonEvents.append(be) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] - events = self.create_common_events(ret, frogpilot_variables, extra_gears=[car.CarState.GearShifter.brake]) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake]) if self.CS.lkas_enabled: events.add(car.CarEvent.EventName.invalidLkasSetting) diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index d064ce8..9bcb23d 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -1,11 +1,9 @@ from dataclasses import dataclass, field -from enum import StrEnum -from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarInfo, CarHarness, CarParts +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarDocs, CarHarness, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -21,29 +19,51 @@ class CarControllerParams: pass -class CAR(StrEnum): - XTRAIL = "NISSAN X-TRAIL 2017" - LEAF = "NISSAN LEAF 2018" - # Leaf with ADAS ECU found behind instrument cluster instead of glovebox - # Currently the only known difference between them is the inverted seatbelt signal. - LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster" - ROGUE = "NISSAN ROGUE 2019" - ALTIMA = "NISSAN ALTIMA 2020" - - @dataclass -class NissanCarInfo(CarInfo): +class NissanCarDocs(CarDocs): package: str = "ProPILOT Assist" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.nissan_a])) -CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { - CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), - CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"), - CAR.LEAF_IC: None, # same platforms - CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), - CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b])), -} +@dataclass(frozen=True) +class NissanCarSpecs(CarSpecs): + centerToFrontRatio: float = 0.44 + steerRatio: float = 17. + + +@dataclass +class NissanPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('nissan_x_trail_2017_generated', None)) + + +class CAR(Platforms): + XTRAIL = NissanPlatformConfig( + "NISSAN X-TRAIL 2017", + [NissanCarDocs("Nissan X-Trail 2017")], + NissanCarSpecs(mass=1610, wheelbase=2.705) + ) + LEAF = NissanPlatformConfig( + "NISSAN LEAF 2018", + [NissanCarDocs("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY")], + NissanCarSpecs(mass=1610, wheelbase=2.705), + dbc_dict('nissan_leaf_2018_generated', None), + ) + # Leaf with ADAS ECU found behind instrument cluster instead of glovebox + # Currently the only known difference between them is the inverted seatbelt signal. + LEAF_IC = LEAF.override(platform_str="NISSAN LEAF 2018 Instrument Cluster", car_docs=[]) + ROGUE = NissanPlatformConfig( + "NISSAN ROGUE 2019", + [NissanCarDocs("Nissan Rogue 2018-20")], + NissanCarSpecs(mass=1610, wheelbase=2.705) + ) + ALTIMA = NissanPlatformConfig( + "NISSAN ALTIMA 2020", + [NissanCarDocs("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b]))], + NissanCarSpecs(mass=1492, wheelbase=2.824) + ) + + +DBC = CAR.create_dbc_map() # Default diagnostic session NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81]) @@ -89,11 +109,3 @@ FW_QUERY_CONFIG = FwQueryConfig( ), ]], ) - -DBC = { - CAR.XTRAIL: dbc_dict('nissan_x_trail_2017_generated', None), - CAR.LEAF: dbc_dict('nissan_leaf_2018_generated', None), - CAR.LEAF_IC: dbc_dict('nissan_leaf_2018_generated', None), - CAR.ROGUE: dbc_dict('nissan_x_trail_2017_generated', None), - CAR.ALTIMA: dbc_dict('nissan_x_trail_2017_generated', None), -} diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 1d6d764..d6f612c 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,9 +1,9 @@ from openpilot.common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.subaru import subarucan -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, \ - CanBus, CarControllerParams, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # involves the total steering angle change rather than rate, but these limits work well for now @@ -11,7 +11,7 @@ MAX_STEER_RATE = 25 # deg/s MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 @@ -42,12 +42,12 @@ class CarController: if not CC.latActive: apply_steer = 0 - if self.CP.carFingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive)) else: apply_steer_req = CC.latActive - if self.CP.carFingerprint in STEER_RATE_LIMITED: + if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED: # Steering rate fault prevention self.steer_rate_counter, apply_steer_req = \ common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req, @@ -74,7 +74,7 @@ class CarController: cruise_brake = CarControllerParams.BRAKE_MIN # *** alerts and pcm cancel *** - if self.CP.carFingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: if self.frame % 5 == 0: # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep # disengage ACC when OP is disengaged @@ -117,8 +117,8 @@ class CarController: self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle)) else: if pcm_cancel_cmd: - if self.CP.carFingerprint not in HYBRID_CARS: - bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main + if not (self.CP.flags & SubaruFlags.HYBRID): + bus = CanBus.alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd)) if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT: diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index bda8342..4ab1e01 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, CanBus, SubaruFlags from openpilot.selfdrive.car import CanSignalRateCalculator @@ -19,17 +19,27 @@ class CarState(CarStateBase): def update(self, cp, cp_cam, cp_body, frogpilot_variables): ret = car.CarState.new_message() - throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] + throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_body.vl["Throttle_Hybrid"] ret.gas = throttle_msg["Throttle_Pedal"] / 255. ret.gasPressed = ret.gas > 1e-5 - if self.car_fingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 0 else: - cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + cp_brakes = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1 - cp_wheels = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam + if not (self.CP.flags & SubaruFlags.HYBRID): + eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"]) + + # if openpilot is controlling long, an eyesight fault is a non-critical fault. otherwise it's an ACC fault + if self.CP.openpilotLongitudinalControl: + ret.carFaultedNonCritical = eyesight_fault + else: + ret.accFaulted = eyesight_fault + + cp_wheels = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp ret.wheelSpeeds = self.get_wheel_speeds( cp_wheels.vl["Wheel_Speeds"]["FL"], cp_wheels.vl["Wheel_Speeds"]["FR"], @@ -48,24 +58,24 @@ class CarState(CarStateBase): ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1) - cp_transmission = cp_body if self.car_fingerprint in HYBRID_CARS else cp + cp_transmission = cp_body if self.CP.flags & SubaruFlags.HYBRID else cp can_gear = int(cp_transmission.vl["Transmission"]["Gear"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] - if self.car_fingerprint not in PREGLOBAL_CARS: + if not (self.CP.flags & SubaruFlags.PREGLOBAL): # ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"]) ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"] ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"] - steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 + steer_threshold = 75 if self.CP.flags & SubaruFlags.PREGLOBAL else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold - cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp - if self.car_fingerprint in HYBRID_CARS: + cp_cruise = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp + if self.CP.flags & SubaruFlags.HYBRID: ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 else: @@ -73,8 +83,8 @@ class CarState(CarStateBase): ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS - if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ - (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): + if (self.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \ + (not (self.CP.flags & SubaruFlags.PREGLOBAL) and cp.vl["Dashlights"]["UNITS"] == 1): ret.cruiseState.speed *= CV.MPH_TO_KPH ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1 @@ -84,8 +94,7 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 - cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam - if self.car_fingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] else: @@ -96,12 +105,12 @@ class CarState(CarStateBase): (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2) self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) - cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + cp_es_brake = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"]) - cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + cp_es_status = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam # TODO: Hybrid cars don't have ES_Distance, need a replacement - if self.car_fingerprint not in HYBRID_CARS: + if not (self.CP.flags & SubaruFlags.HYBRID): # 8 is known AEB, there are a few other values related to AEB we ignore ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \ (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0) @@ -109,13 +118,16 @@ class CarState(CarStateBase): self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"]) self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"]) - if self.car_fingerprint not in HYBRID_CARS: + if not (self.CP.flags & SubaruFlags.HYBRID): self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"] + return ret @staticmethod @@ -125,7 +137,7 @@ class CarState(CarStateBase): ("Brake_Status", 50), ] - if CP.carFingerprint not in HYBRID_CARS: + if not (CP.flags & SubaruFlags.HYBRID): messages.append(("CruiseControl", 20)) return messages @@ -136,7 +148,7 @@ class CarState(CarStateBase): ("ES_Brake", 20), ] - if CP.carFingerprint not in HYBRID_CARS: + if not (CP.flags & SubaruFlags.HYBRID): messages += [ ("ES_Distance", 20), ("ES_Status", 20) @@ -164,7 +176,7 @@ class CarState(CarStateBase): ("Brake_Pedal", 50), ] - if CP.carFingerprint not in HYBRID_CARS: + if not (CP.flags & SubaruFlags.HYBRID): messages += [ ("Throttle", 100), ("Transmission", 100) @@ -173,8 +185,8 @@ class CarState(CarStateBase): if CP.enableBsm: messages.append(("BSD_RCTA", 17)) - if CP.carFingerprint not in PREGLOBAL_CARS: - if CP.carFingerprint not in GLOBAL_GEN2: + if not (CP.flags & SubaruFlags.PREGLOBAL): + if not (CP.flags & SubaruFlags.GLOBAL_GEN2): messages += CarState.get_common_global_body_messages(CP) else: messages += CarState.get_common_preglobal_body_messages() @@ -183,7 +195,7 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): - if CP.carFingerprint in PREGLOBAL_CARS: + if CP.flags & SubaruFlags.PREGLOBAL: messages = [ ("ES_DashStatus", 20), ("ES_Distance", 20), @@ -194,7 +206,7 @@ class CarState(CarStateBase): ("ES_LKAS_State", 10), ] - if CP.carFingerprint not in GLOBAL_GEN2: + if not (CP.flags & SubaruFlags.GLOBAL_GEN2): messages += CarState.get_common_global_es_messages(CP) if CP.flags & SubaruFlags.SEND_INFOTAINMENT: @@ -206,15 +218,14 @@ class CarState(CarStateBase): def get_body_can_parser(CP): messages = [] - if CP.carFingerprint in GLOBAL_GEN2: + if CP.flags & SubaruFlags.GLOBAL_GEN2: messages += CarState.get_common_global_body_messages(CP) messages += CarState.get_common_global_es_messages(CP) - if CP.carFingerprint in HYBRID_CARS: + if CP.flags & SubaruFlags.HYBRID: messages += [ ("Throttle_Hybrid", 40), ("Transmission", 100) ] return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt) - diff --git a/selfdrive/car/subaru/fingerprints.py b/selfdrive/car/subaru/fingerprints.py index 90fa609..9f6177b 100644 --- a/selfdrive/car/subaru/fingerprints.py +++ b/selfdrive/car/subaru/fingerprints.py @@ -26,6 +26,7 @@ FW_VERSIONS = { b'\xd1,\xa0q\x07', ], (Ecu.transmission, 0x7e1, None): [ + b'\x00>\xf0\x00\x00', b'\x00\xfe\xf7\x00\x00', b'\x01\xfe\xf7\x00\x00', b'\x01\xfe\xf9\x00\x00', diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index e1c53ab..66c63a5 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,15 +1,16 @@ -from cereal import car +from cereal import car, custom from panda import Panda -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags +from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): crosstrek_torque_increase = params.get_bool("CrosstrekTorque") ret.carName = "subaru" @@ -18,112 +19,78 @@ class CarInterface(CarInterfaceBase): # - replacement for ES_Distance so we can cancel the cruise control # - to find the Cruise_Activated bit from the car # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc) - ret.dashcamOnly = candidate in (LKAS_ANGLE | HYBRID_CARS) + ret.dashcamOnly = bool(ret.flags & (SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID)) ret.autoResumeSng = False # Detect infotainment message sent from the camera - if candidate not in PREGLOBAL_CARS and 0x323 in fingerprint[2]: + if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]: ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value - if candidate in PREGLOBAL_CARS: + if ret.flags & SubaruFlags.PREGLOBAL: ret.enableBsm = 0x25c in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)] else: ret.enableBsm = 0x228 in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] - if candidate in GLOBAL_GEN2: + if ret.flags & SubaruFlags.GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 - if candidate in LKAS_ANGLE: + if ret.flags & SubaruFlags.LKAS_ANGLE: ret.steerControlType = car.CarParams.SteerControlType.angle else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) if candidate in (CAR.ASCENT, CAR.ASCENT_2023): - ret.mass = 2031. - ret.wheelbase = 2.89 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 13.5 - ret.steerActuatorDelay = 0.3 # end-to-end angle controller + ret.steerActuatorDelay = 0.3 # end-to-end angle controller ret.lateralTuning.init('pid') - ret.lateralTuning.pid.kf = 0.00003333 if crosstrek_torque_increase else 0.00003 + ret.lateralTuning.pid.kf = 0.00003 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.133, 0.2], [0.0133, 0.02]] if crosstrek_torque_increase else [[0.0025, 0.1], [0.00025, 0.01]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]] elif candidate == CAR.IMPREZA: - ret.mass = 1568. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 15 - ret.steerActuatorDelay = 0.4 # end-to-end angle controller + ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.lateralTuning.init('pid') - ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kf = 0.00003333 if crosstrek_torque_increase else 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.133, 0.2], [0.0133, 0.02]] if crosstrek_torque_increase else [[0.2, 0.3], [0.02, 0.03]] elif candidate == CAR.IMPREZA_2020: - ret.mass = 1480. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 # learned, 14 stock ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] elif candidate == CAR.CROSSTREK_HYBRID: - ret.mass = 1668. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 ret.steerActuatorDelay = 0.1 elif candidate in (CAR.FORESTER, CAR.FORESTER_2022, CAR.FORESTER_HYBRID): - ret.mass = 1568. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 # learned, 14 stock ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.000038 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] elif candidate in (CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023): - ret.mass = 1568. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 ret.steerActuatorDelay = 0.1 elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018): ret.safetyConfigs[0].safetyParam = Panda.FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE # Outback 2018-2019 and Forester have reversed driver torque signal - ret.mass = 1568 - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 20 # learned, 14 stock elif candidate == CAR.LEGACY_PREGLOBAL: - ret.mass = 1568 - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 12.5 # 14.5 stock ret.steerActuatorDelay = 0.15 elif candidate == CAR.OUTBACK_PREGLOBAL: - ret.mass = 1568 - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 20 # learned, 14 stock + pass else: raise ValueError(f"unknown car: {candidate}") - ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS) - ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable and not params.get_bool("DisableOpenpilotLongitudinal") + ret.experimentalLongitudinalAvailable = not (ret.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.PREGLOBAL | + SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID)) + ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable - if candidate in GLOBAL_GEN2 and ret.openpilotLongitudinalControl: + if ret.flags & SubaruFlags.GLOBAL_GEN2 and ret.openpilotLongitudinalControl: ret.flags |= SubaruFlags.DISABLE_EYESIGHT.value if ret.openpilotLongitudinalControl: @@ -142,7 +109,11 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables) - ret.events = self.create_common_events(ret, frogpilot_variables).to_msg() + ret.buttonEvents = [ + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + + ret.events = self.create_common_events(ret).to_msg() return ret diff --git a/selfdrive/car/subaru/tests/test_subaru.py b/selfdrive/car/subaru/tests/test_subaru.py new file mode 100644 index 0000000..c8cdf66 --- /dev/null +++ b/selfdrive/car/subaru/tests/test_subaru.py @@ -0,0 +1,20 @@ +from cereal import car +import unittest +from openpilot.selfdrive.car.subaru.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu + +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + + +class TestSubaruFingerprint(unittest.TestCase): + def test_fw_version_format(self): + for platform, fws_per_ecu in FW_VERSIONS.items(): + for (ecu, _, _), fws in fws_per_ecu.items(): + fw_size = len(fws[0]) + for fw in fws: + self.assertEqual(len(fw), fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index bda859c..3fe4843 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -1,12 +1,11 @@ from dataclasses import dataclass, field -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Union +from enum import Enum, IntFlag from cereal import car from panda.python import uds from openpilot.common.params import Params -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Tool, Column +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -21,7 +20,7 @@ class CarControllerParams: self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily self.STEER_DRIVER_FACTOR = 1 # from dbc - if CP.carFingerprint in GLOBAL_GEN2: + if CP.flags & SubaruFlags.GLOBAL_GEN2: self.STEER_MAX = 1000 self.STEER_DELTA_UP = 40 self.STEER_DELTA_DOWN = 40 @@ -57,9 +56,20 @@ class CarControllerParams: class SubaruFlags(IntFlag): + # Detected flags SEND_INFOTAINMENT = 1 DISABLE_EYESIGHT = 2 + # Static flags + GLOBAL_GEN2 = 4 + + # Cars that temporarily fault when steering angle rate is greater than some threshold. + # Appears to be all torque-based cars produced around 2019 - present + STEER_RATE_LIMITED = 8 + PREGLOBAL = 16 + HYBRID = 32 + LKAS_ANGLE = 64 + GLOBAL_ES_ADDR = 0x787 GEN2_ES_BUTTONS_DID = b'\x11\x30' @@ -71,27 +81,6 @@ class CanBus: camera = 2 -class CAR(StrEnum): - # Global platform - ASCENT = "SUBARU ASCENT LIMITED 2019" - ASCENT_2023 = "SUBARU ASCENT 2023" - IMPREZA = "SUBARU IMPREZA LIMITED 2019" - IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020" - FORESTER = "SUBARU FORESTER 2019" - OUTBACK = "SUBARU OUTBACK 6TH GEN" - CROSSTREK_HYBRID = "SUBARU CROSSTREK HYBRID 2020" - FORESTER_HYBRID = "SUBARU FORESTER HYBRID 2020" - LEGACY = "SUBARU LEGACY 7TH GEN" - FORESTER_2022 = "SUBARU FORESTER 2022" - OUTBACK_2023 = "SUBARU OUTBACK 7TH GEN" - - # Pre-global - FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018" - LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018" - OUTBACK_PREGLOBAL = "SUBARU OUTBACK 2015 - 2017" - OUTBACK_PREGLOBAL_2018 = "SUBARU OUTBACK 2018 - 2019" - - class Footnote(Enum): GLOBAL = CarFootnote( "In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.", @@ -102,10 +91,10 @@ class Footnote(Enum): @dataclass -class SubaruCarInfo(CarInfo): +class SubaruCarDocs(CarDocs): package: str = "EyeSight Driver Assistance" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.subaru_a])) - footnotes: List[Enum] = field(default_factory=lambda: [Footnote.GLOBAL]) + footnotes: list[Enum] = field(default_factory=lambda: [Footnote.GLOBAL]) def init_make(self, CP: car.CarParams): self.car_parts.parts.extend([Tool.socket_8mm_deep, Tool.pry_tool]) @@ -113,68 +102,180 @@ class SubaruCarInfo(CarInfo): if CP.experimentalLongitudinalAvailable: self.footnotes.append(Footnote.EXP_LONG) -CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { - CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"), - CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b])), - CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b])), - CAR.IMPREZA: [ - SubaruCarInfo("Subaru Impreza 2017-19"), - SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), - SubaruCarInfo("Subaru XV 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), - ], - CAR.IMPREZA_2020: [ - SubaruCarInfo("Subaru Impreza 2020-22"), - SubaruCarInfo("Subaru Crosstrek 2020-23"), - SubaruCarInfo("Subaru XV 2020-21"), - ], + +@dataclass +class SubaruPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('subaru_global_2017_generated', None)) + + def init(self): + if self.flags & SubaruFlags.HYBRID: + self.dbc_dict = dbc_dict('subaru_global_2020_hybrid_generated', None) + + +@dataclass +class SubaruGen2PlatformConfig(SubaruPlatformConfig): + def init(self): + super().init() + self.flags |= SubaruFlags.GLOBAL_GEN2 + if not (self.flags & SubaruFlags.LKAS_ANGLE): + self.flags |= SubaruFlags.STEER_RATE_LIMITED + + +class CAR(Platforms): + # Global platform + ASCENT = SubaruPlatformConfig( + "SUBARU ASCENT LIMITED 2019", + [SubaruCarDocs("Subaru Ascent 2019-21", "All")], + CarSpecs(mass=2031, wheelbase=2.89, steerRatio=13.5), + ) + OUTBACK = SubaruGen2PlatformConfig( + "SUBARU OUTBACK 6TH GEN", + [SubaruCarDocs("Subaru Outback 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b]))], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=17), + ) + LEGACY = SubaruGen2PlatformConfig( + "SUBARU LEGACY 7TH GEN", + [SubaruCarDocs("Subaru Legacy 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b]))], + OUTBACK.specs, + ) + IMPREZA = SubaruPlatformConfig( + "SUBARU IMPREZA LIMITED 2019", + [ + SubaruCarDocs("Subaru Impreza 2017-19"), + SubaruCarDocs("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), + SubaruCarDocs("Subaru XV 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), + ], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=15), + ) + IMPREZA_2020 = SubaruPlatformConfig( + "SUBARU IMPREZA SPORT 2020", + [ + SubaruCarDocs("Subaru Impreza 2020-22"), + SubaruCarDocs("Subaru Crosstrek 2020-23"), + SubaruCarDocs("Subaru XV 2020-21"), + ], + CarSpecs(mass=1480, wheelbase=2.67, steerRatio=17), + flags=SubaruFlags.STEER_RATE_LIMITED, + ) # TODO: is there an XV and Impreza too? - CAR.CROSSTREK_HYBRID: SubaruCarInfo("Subaru Crosstrek Hybrid 2020", car_parts=CarParts.common([CarHarness.subaru_b])), - CAR.FORESTER_HYBRID: SubaruCarInfo("Subaru Forester Hybrid 2020"), - CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), - CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), - CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), - CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), - CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"), - CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022-24", "All", car_parts=CarParts.common([CarHarness.subaru_c])), - CAR.OUTBACK_2023: SubaruCarInfo("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])), - CAR.ASCENT_2023: SubaruCarInfo("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])), -} + CROSSTREK_HYBRID = SubaruPlatformConfig( + "SUBARU CROSSTREK HYBRID 2020", + [SubaruCarDocs("Subaru Crosstrek Hybrid 2020", car_parts=CarParts.common([CarHarness.subaru_b]))], + CarSpecs(mass=1668, wheelbase=2.67, steerRatio=17), + flags=SubaruFlags.HYBRID, + ) + FORESTER = SubaruPlatformConfig( + "SUBARU FORESTER 2019", + [SubaruCarDocs("Subaru Forester 2019-21", "All")], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=17), + flags=SubaruFlags.STEER_RATE_LIMITED, + ) + FORESTER_HYBRID = SubaruPlatformConfig( + "SUBARU FORESTER HYBRID 2020", + [SubaruCarDocs("Subaru Forester Hybrid 2020")], + FORESTER.specs, + flags=SubaruFlags.HYBRID, + ) + # Pre-global + FORESTER_PREGLOBAL = SubaruPlatformConfig( + "SUBARU FORESTER 2017 - 2018", + [SubaruCarDocs("Subaru Forester 2017-18")], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=20), + dbc_dict('subaru_forester_2017_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + LEGACY_PREGLOBAL = SubaruPlatformConfig( + "SUBARU LEGACY 2015 - 2018", + [SubaruCarDocs("Subaru Legacy 2015-18")], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=12.5), + dbc_dict('subaru_outback_2015_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + OUTBACK_PREGLOBAL = SubaruPlatformConfig( + "SUBARU OUTBACK 2015 - 2017", + [SubaruCarDocs("Subaru Outback 2015-17")], + FORESTER_PREGLOBAL.specs, + dbc_dict('subaru_outback_2015_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + OUTBACK_PREGLOBAL_2018 = SubaruPlatformConfig( + "SUBARU OUTBACK 2018 - 2019", + [SubaruCarDocs("Subaru Outback 2018-19")], + FORESTER_PREGLOBAL.specs, + dbc_dict('subaru_outback_2019_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + # Angle LKAS + FORESTER_2022 = SubaruPlatformConfig( + "SUBARU FORESTER 2022", + [SubaruCarDocs("Subaru Forester 2022-24", "All", car_parts=CarParts.common([CarHarness.subaru_c]))], + FORESTER.specs, + flags=SubaruFlags.LKAS_ANGLE, + ) + OUTBACK_2023 = SubaruGen2PlatformConfig( + "SUBARU OUTBACK 7TH GEN", + [SubaruCarDocs("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + OUTBACK.specs, + flags=SubaruFlags.LKAS_ANGLE, + ) + ASCENT_2023 = SubaruGen2PlatformConfig( + "SUBARU ASCENT 2023", + [SubaruCarDocs("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + ASCENT.specs, + flags=SubaruFlags.LKAS_ANGLE, + ) -LKAS_ANGLE = {CAR.FORESTER_2022, CAR.OUTBACK_2023, CAR.ASCENT_2023} -GLOBAL_GEN2 = {CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023, CAR.ASCENT_2023} -PREGLOBAL_CARS = {CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018} -HYBRID_CARS = {CAR.CROSSTREK_HYBRID, CAR.FORESTER_HYBRID} - -# Cars that temporarily fault when steering angle rate is greater than some threshold. -# Appears to be all torque-based cars produced around 2019 - present -STEER_RATE_LIMITED = GLOBAL_GEN2 | {CAR.IMPREZA_2020, CAR.FORESTER} SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) +# The EyeSight ECU takes 10s to respond to SUBARU_VERSION_REQUEST properly, +# log this alternate manufacturer-specific query +SUBARU_ALT_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf100) +SUBARU_ALT_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(0xf100) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], + logging=True, ), + # Non-OBD requests # Some Eyesight modules fail on TESTER_PRESENT_REQUEST # TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars Request( [SUBARU_VERSION_REQUEST], [SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.fwdCamera], + bus=0, + ), + Request( + [SUBARU_ALT_VERSION_REQUEST], + [SUBARU_ALT_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera], + bus=0, + logging=True, + ), + Request( + [StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], + [StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera], + bus=0, + logging=True, ), - # Non-OBD requests Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], bus=0, ), + # GEN2 powertrain bus query Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], @@ -185,24 +286,11 @@ FW_QUERY_CONFIG = FwQueryConfig( ], # We don't get the EPS from non-OBD queries on GEN2 cars. Note that we still attempt to match when it exists non_essential_ecus={ - Ecu.eps: list(GLOBAL_GEN2), + Ecu.eps: list(CAR.with_flags(SubaruFlags.GLOBAL_GEN2)), } ) -DBC = { - CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None), - CAR.ASCENT_2023: dbc_dict('subaru_global_2017_generated', None), - CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), - CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None), - CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None), - CAR.CROSSTREK_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None), - CAR.OUTBACK_2023: dbc_dict('subaru_global_2017_generated', None), - CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), - CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), - CAR.OUTBACK_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), - CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None), -} +DBC = CAR.create_dbc_map() + +if __name__ == "__main__": + CAR.print_debug(SubaruFlags) diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 9d5617b..127b3f9 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -1,11 +1,12 @@ from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.frame = 0 diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index ed59693..fb3e6af 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -2,7 +2,7 @@ import copy from collections import deque from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS +from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine @@ -37,13 +37,15 @@ class CarState(CarStateBase): ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) # Steering wheel - self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"] - self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None) - steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None) + epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.MODELS_RAVEN else cp.vl["EPAS_sysStatus"] - ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"] + self.hands_on_level = epas_status["EPAS_handsOnLevel"] + self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None) + steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None) + + ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"] ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate - ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"] + ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"] ret.steeringPressed = (self.hands_on_level > 0) ret.steerFaultPermanent = steer_status == "EAC_FAULT" ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON")) @@ -85,7 +87,10 @@ class CarState(CarStateBase): ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) # Seatbelt - ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) + if self.CP.carFingerprint == CAR.MODELS_RAVEN: + ret.seatbeltUnlatched = (cp.vl["DriverSeat"]["buckleStatus"] != 1) + else: + ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) # TODO: blindspot @@ -111,9 +116,14 @@ class CarState(CarStateBase): ("DI_state", 10), ("STW_ACTN_RQ", 10), ("GTW_carState", 10), - ("SDM1", 10), ("BrakeMessage", 50), ] + + if CP.carFingerprint == CAR.MODELS_RAVEN: + messages.append(("DriverSeat", 20)) + else: + messages.append(("SDM1", 10)) + return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis) @staticmethod @@ -122,4 +132,8 @@ class CarState(CarStateBase): # sig_address, frequency ("DAS_control", 40), ] + + if CP.carFingerprint == CAR.MODELS_RAVEN: + messages.append(("EPAS3P_sysStatus", 100)) + return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis) diff --git a/selfdrive/car/tesla/fingerprints.py b/selfdrive/car/tesla/fingerprints.py index 772ca59..9b6f386 100644 --- a/selfdrive/car/tesla/fingerprints.py +++ b/selfdrive/car/tesla/fingerprints.py @@ -25,4 +25,15 @@ FW_VERSIONS = { b'\x10#\x01', ], }, + CAR.MODELS_RAVEN: { + (Ecu.electricBrakeBooster, 0x64d, None): [ + b'1037123-00-A', + ], + (Ecu.fwdRadar, 0x671, None): [ + b'\x01\x00\x99\x02\x01\x00\x10\x00\x00AP8.3.03\x00\x10', + ], + (Ecu.eps, 0x730, None): [ + b'SX_0.0.0 (99),SR013.7', + ], + }, } diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index ca06171..74bb61a 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -8,7 +8,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, @@ -28,33 +28,26 @@ class CarInterface(CarInterfaceBase): # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus # If so, we assume that it is connected to the longitudinal harness. + flags = (Panda.FLAG_TESLA_RAVEN if candidate == CAR.MODELS_RAVEN else 0) if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()): - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = not disable_openpilot_long + flags |= Panda.FLAG_TESLA_LONG_CONTROL ret.safetyConfigs = [ - get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL), - get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN), + get_safety_config(car.CarParams.SafetyModel.tesla, flags), + get_safety_config(car.CarParams.SafetyModel.tesla, flags | Panda.FLAG_TESLA_POWERTRAIN), ] else: ret.openpilotLongitudinalControl = False - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)] ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.25 - - if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): - ret.mass = 2100. - ret.wheelbase = 2.959 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 15.0 - else: - raise ValueError(f"Unsupported car: {candidate}") - return ret def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) - ret.events = self.create_common_events(ret, frogpilot_variables).to_msg() + ret.events = self.create_common_events(ret).to_msg() return ret diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index b3e7c7f..599ab31 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -1,38 +1,33 @@ #!/usr/bin/env python3 from cereal import car from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.tesla.values import DBC, CANBUS +from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS from openpilot.selfdrive.car.interfaces import RadarInterfaceBase -RADAR_MSGS_A = list(range(0x310, 0x36E, 3)) -RADAR_MSGS_B = list(range(0x311, 0x36F, 3)) -NUM_POINTS = len(RADAR_MSGS_A) - -def get_radar_can_parser(CP): - # Status messages - messages = [ - ('TeslaRadarSguInfo', 10), - ] - - # Radar tracks. There are also raw point clouds available, - # we don't use those. - for i in range(NUM_POINTS): - msg_id_a = RADAR_MSGS_A[i] - msg_id_b = RADAR_MSGS_B[i] - messages.extend([ - (msg_id_a, 8), - (msg_id_b, 8), - ]) - - return CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar) class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) - self.rcp = get_radar_can_parser(CP) + self.CP = CP + + if CP.carFingerprint == CAR.MODELS_RAVEN: + messages = [('RadarStatus', 16)] + self.num_points = 40 + self.trigger_msg = 1119 + else: + messages = [('TeslaRadarSguInfo', 10)] + self.num_points = 32 + self.trigger_msg = 878 + + for i in range(self.num_points): + messages.extend([ + (f'RadarPoint{i}_A', 16), + (f'RadarPoint{i}_B', 16), + ]) + + self.rcp = CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar) self.updated_messages = set() self.track_id = 0 - self.trigger_msg = RADAR_MSGS_B[-1] def update(self, can_strings): if self.rcp is None: @@ -48,17 +43,24 @@ class RadarInterface(RadarInterfaceBase): # Errors errors = [] - sgu_info = self.rcp.vl['TeslaRadarSguInfo'] if not self.rcp.can_valid: errors.append('canError') - if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']: - errors.append('fault') + + if self.CP.carFingerprint == CAR.MODELS_RAVEN: + radar_status = self.rcp.vl['RadarStatus'] + if radar_status['sensorBlocked'] or radar_status['shortTermUnavailable'] or radar_status['vehDynamicsError']: + errors.append('fault') + else: + radar_status = self.rcp.vl['TeslaRadarSguInfo'] + if radar_status['RADC_HWFail'] or radar_status['RADC_SGUFail'] or radar_status['RADC_SensorDirty']: + errors.append('fault') + ret.errors = errors # Radar tracks - for i in range(NUM_POINTS): - msg_a = self.rcp.vl[RADAR_MSGS_A[i]] - msg_b = self.rcp.vl[RADAR_MSGS_B[i]] + for i in range(self.num_points): + msg_a = self.rcp.vl[f'RadarPoint{i}_A'] + msg_b = self.rcp.vl[f'RadarPoint{i}_B'] # Make sure msg A and B are together if msg_a['Index'] != msg_b['Index2']: diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 12877f1..74f38f2 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -1,32 +1,33 @@ from collections import namedtuple -from enum import StrEnum -from typing import Dict, List, Union from cereal import car -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarInfo +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarDocs from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) - -class CAR(StrEnum): - AP1_MODELS = 'TESLA AP1 MODEL S' - AP2_MODELS = 'TESLA AP2 MODEL S' - - -CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { - CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"), - CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"), -} - - -DBC = { - CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), - CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), -} +class CAR(Platforms): + AP1_MODELS = PlatformConfig( + 'TESLA AP1 MODEL S', + [CarDocs("Tesla AP1 Model S", "All")], + CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0), + dbc_dict('tesla_powertrain', 'tesla_radar_bosch_generated', chassis_dbc='tesla_can') + ) + AP2_MODELS = PlatformConfig( + 'TESLA AP2 MODEL S', + [CarDocs("Tesla AP2 Model S", "All")], + AP1_MODELS.specs, + AP1_MODELS.dbc_dict + ) + MODELS_RAVEN = PlatformConfig( + 'TESLA MODEL S RAVEN', + [CarDocs("Tesla Model S Raven", "All")], + AP1_MODELS.specs, + dbc_dict('tesla_powertrain', 'tesla_radar_continental_generated', chassis_dbc='tesla_can') + ) FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -37,6 +38,13 @@ FW_QUERY_CONFIG = FwQueryConfig( rx_offset=0x08, bus=0, ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.eps], + rx_offset=0x08, + bus=0, + ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], @@ -47,7 +55,6 @@ FW_QUERY_CONFIG = FwQueryConfig( ] ) - class CANBUS: # Lateral harness chassis = 0 @@ -89,3 +96,6 @@ class CarControllerParams: def __init__(self, CP): pass + + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index a95c13e..265f052 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.car.nissan.values import CAR as NISSAN from openpilot.selfdrive.car.mazda.values import CAR as MAZDA from openpilot.selfdrive.car.subaru.values import CAR as SUBARU from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.values import Platform from openpilot.selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN from openpilot.selfdrive.car.tesla.values import CAR as TESLA from openpilot.selfdrive.car.body.values import CAR as COMMA @@ -29,7 +30,7 @@ non_tested_cars = [ class CarTestRoute(NamedTuple): route: str - car_model: str | None + car_model: Platform | None segment: int | None = None @@ -229,7 +230,7 @@ routes = [ CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1), CarTestRoute("2c68dda277d887ac|2021-05-11--15-22-20", VOLKSWAGEN.ATLAS_MK1), - #CarTestRoute("ffcd23abbbd02219|2024-02-28--14-59-38", VOLKSWAGEN.CADDY_MK3), + CarTestRoute("ffcd23abbbd02219|2024-02-28--14-59-38", VOLKSWAGEN.CADDY_MK3), CarTestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.GOLF_MK7), # Stock ACC CarTestRoute("3cfdec54aa035f3f|2022-10-13--14-58-58", VOLKSWAGEN.GOLF_MK7), # openpilot longitudinal CarTestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.JETTA_MK7), @@ -289,7 +290,7 @@ routes = [ CarTestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.AP1_MODELS), CarTestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.AP2_MODELS), - #CarTestRoute("66c1699b7697267d/2024-03-03--13-09-53", TESLA.MODELS_RAVEN), + CarTestRoute("66c1699b7697267d/2024-03-03--13-09-53", TESLA.MODELS_RAVEN), # Segments that test specific issues # Controls mismatch due to interceptor threshold diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 6122577..02a8d60 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -9,7 +9,6 @@ from parameterized import parameterized from cereal import car, messaging from openpilot.common.realtime import DT_CTRL -from openpilot.common.params import Params from openpilot.selfdrive.car import gen_empty_fingerprint from openpilot.selfdrive.car.car_helpers import interfaces from openpilot.selfdrive.car.fingerprints import all_known_cars @@ -19,7 +18,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl -from openpilot.selfdrive.controls.controlsd import Controls from openpilot.selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator ALL_ECUS = list({ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()}) @@ -46,6 +44,7 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict: params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']] return params + class TestCarInterfaces(unittest.TestCase): # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause # many generated examples to overrun when max_examples > ~20, don't use it @@ -57,11 +56,9 @@ class TestCarInterfaces(unittest.TestCase): CarInterface, CarController, CarState = interfaces[car_name] args = get_fuzzy_car_interface_args(data.draw) - params = Params() - car_params = CarInterface.get_params(params, car_name, args['fingerprints'], args['car_fw'], + car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], experimental_long=args['experimental_long'], docs=False) - car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface @@ -97,19 +94,18 @@ class TestCarInterfaces(unittest.TestCase): # Run car interface now_nanos = 0 CC = car.CarControl.new_message(**cc_msg) - controls = Controls(CI=car_interface) for _ in range(10): - car_interface.update(CC, [], controls.frogpilot_variables) - car_interface.apply(CC, now_nanos, controls.frogpilot_variables) - car_interface.apply(CC, now_nanos, controls.frogpilot_variables) + car_interface.update(CC, []) + car_interface.apply(CC, now_nanos) + car_interface.apply(CC, now_nanos) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) CC.enabled = True for _ in range(10): - car_interface.update(CC, [], controls.frogpilot_variables) - car_interface.apply(CC, now_nanos, controls.frogpilot_variables) - car_interface.apply(CC, now_nanos, controls.frogpilot_variables) + car_interface.update(CC, []) + car_interface.apply(CC, now_nanos) + car_interface.apply(CC, now_nanos) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index 0ee35dd..7f88dba 100644 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -6,18 +6,18 @@ import unittest from openpilot.common.basedir import BASEDIR from openpilot.selfdrive.car.car_helpers import interfaces -from openpilot.selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_info +from openpilot.selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_docs from openpilot.selfdrive.car.docs_definitions import Cable, Column, PartType, Star from openpilot.selfdrive.car.honda.values import CAR as HONDA from openpilot.selfdrive.car.values import PLATFORMS -from openpilot.selfdrive.debug.dump_car_info import dump_car_info -from openpilot.selfdrive.debug.print_docs_diff import print_car_info_diff +from openpilot.selfdrive.debug.dump_car_docs import dump_car_docs +from openpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff class TestCarDocs(unittest.TestCase): @classmethod def setUpClass(cls): - cls.all_cars = get_all_car_info() + cls.all_cars = get_all_car_docs() def test_generator(self): generated_cars_md = generate_cars_md(self.all_cars, CARS_MD_TEMPLATE) @@ -29,24 +29,24 @@ class TestCarDocs(unittest.TestCase): def test_docs_diff(self): dump_path = os.path.join(BASEDIR, "selfdrive", "car", "tests", "cars_dump") - dump_car_info(dump_path) - print_car_info_diff(dump_path) + dump_car_docs(dump_path) + print_car_docs_diff(dump_path) os.remove(dump_path) def test_duplicate_years(self): make_model_years = defaultdict(list) for car in self.all_cars: - with self.subTest(car_info_name=car.name): + with self.subTest(car_docs_name=car.name): make_model = (car.make, car.model) for year in car.year_list: self.assertNotIn(year, make_model_years[make_model], f"{car.name}: Duplicate model year") make_model_years[make_model].append(year) - def test_missing_car_info(self): - all_car_info_platforms = [name for name, config in PLATFORMS.items()] + def test_missing_car_docs(self): + all_car_docs_platforms = [name for name, config in PLATFORMS.items()] for platform in sorted(interfaces.keys()): with self.subTest(platform=platform): - self.assertTrue(platform in all_car_info_platforms, f"Platform: {platform} doesn't have a CarInfo entry") + self.assertTrue(platform in all_car_docs_platforms, f"Platform: {platform} doesn't have a CarDocs entry") def test_naming_conventions(self): # Asserts market-standard car naming conventions by brand diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index b9eadc8..d9bea3b 100644 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -263,7 +263,7 @@ class TestFwFingerprintTiming(unittest.TestCase): print(f'get_vin {name} case, query time={self.total_time / self.N} seconds') def test_fw_query_timing(self): - total_ref_time = {1: 8.4, 2: 9.3} + total_ref_time = {1: 8.6, 2: 9.5} brand_ref_times = { 1: { 'gm': 1.0, @@ -274,7 +274,7 @@ class TestFwFingerprintTiming(unittest.TestCase): 'hyundai': 1.05, 'mazda': 0.1, 'nissan': 0.8, - 'subaru': 0.45, + 'subaru': 0.65, 'tesla': 0.3, 'toyota': 1.6, 'volkswagen': 0.65, diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index b7d20e5..81dfe19 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -19,6 +19,7 @@ from openpilot.selfdrive.car.fingerprints import all_known_cars from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, interfaces from openpilot.selfdrive.car.honda.values import CAR as HONDA, HondaFlags from openpilot.selfdrive.car.tests.routes import non_tested_cars, routes, CarTestRoute +from openpilot.selfdrive.car.values import Platform from openpilot.selfdrive.controls.controlsd import Controls from openpilot.selfdrive.test.helpers import read_segment_list from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT @@ -64,7 +65,7 @@ def get_test_cases() -> list[tuple[str, CarTestRoute | None]]: @pytest.mark.slow @pytest.mark.shared_download_cache class TestCarModelBase(unittest.TestCase): - car_model: str | None = None + platform: Platform | None = None test_route: CarTestRoute | None = None test_route_on_bucket: bool = True # whether the route is on the preserved CI bucket @@ -93,8 +94,8 @@ class TestCarModelBase(unittest.TestCase): car_fw = msg.carParams.carFw if msg.carParams.openpilotLongitudinalControl: experimental_long = True - if cls.car_model is None and not cls.ci: - cls.car_model = msg.carParams.carFingerprint + if cls.platform is None and not cls.ci: + cls.platform = msg.carParams.carFingerprint # Log which can frame the panda safety mode left ELM327, for CAN validity checks elif msg.which() == 'pandaStates': @@ -155,15 +156,11 @@ class TestCarModelBase(unittest.TestCase): if cls.__name__ == 'TestCarModel' or cls.__name__.endswith('Base'): raise unittest.SkipTest - if 'FILTER' in os.environ: - if not cls.car_model.startswith(tuple(os.environ.get('FILTER').split(','))): - raise unittest.SkipTest - if cls.test_route is None: - if cls.car_model in non_tested_cars: - print(f"Skipping tests for {cls.car_model}: missing route") + if cls.platform in non_tested_cars: + print(f"Skipping tests for {cls.platform}: missing route") raise unittest.SkipTest - raise Exception(f"missing test route for {cls.car_model}") + raise Exception(f"missing test route for {cls.platform}") car_fw, can_msgs, experimental_long = cls.get_testing_data() @@ -172,10 +169,10 @@ class TestCarModelBase(unittest.TestCase): cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime) - cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model] - cls.CP = cls.CarInterface.get_params(cls.car_model, cls.fingerprint, car_fw, experimental_long, docs=False) + cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.platform] + cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, experimental_long, docs=False) assert cls.CP - assert cls.CP.carFingerprint == cls.car_model + assert cls.CP.carFingerprint == cls.platform os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT @@ -478,7 +475,7 @@ class TestCarModelBase(unittest.TestCase): "This is fine to fail for WIP car ports, just let us know and we can upload your routes to the CI bucket.") -@parameterized_class(('car_model', 'test_route'), get_test_cases()) +@parameterized_class(('platform', 'test_route'), get_test_cases()) @pytest.mark.xdist_group_class_property('test_route') class TestCarModel(TestCarModelBase): pass diff --git a/selfdrive/car/torque_data/neural_ff_weights.json b/selfdrive/car/torque_data/neural_ff_weights.json index c526f07..95c6d22 100644 --- a/selfdrive/car/torque_data/neural_ff_weights.json +++ b/selfdrive/car/torque_data/neural_ff_weights.json @@ -1 +1,2 @@ -{"CHEVROLET BOLT EUV 2022": {"w_1": [[0.3452189564704895, -0.15614677965641022, -0.04062516987323761, -0.5960758328437805, 0.3211185932159424, 0.31732726097106934, -0.04430829733610153, -0.37327295541763306, -0.14118380844593048, 0.12712529301643372, 0.2641555070877075, -0.3451094627380371, -0.005127656273543835, 0.6185108423233032, 0.03725295141339302, 0.3763789236545563], [-0.0708412230014801, 0.3667356073856354, 0.031383827328681946, 0.1740853488445282, -0.04695861041545868, 0.018055908381938934, 0.009072160348296165, -0.23640218377113342, -0.10362917929887772, 0.022628149017691612, -0.224413201212883, 0.20718418061733246, -0.016947750002145767, -0.3872031271457672, -0.15500062704086304, -0.06375953555107117], [-0.0838046595454216, -0.0242826659232378, -0.07765661180019379, 0.028858814388513565, -0.09516210108995438, 0.008368706330657005, 0.1689300835132599, 0.015036891214549541, -0.15121428668498993, 0.1388195902109146, 0.11486363410949707, 0.0651545450091362, 0.13559958338737488, 0.04300367832183838, -0.13856294751167297, -0.058136988431215286], [-0.006249868310987949, 0.08809533715248108, -0.040690965950489044, 0.02359287068247795, -0.00766348373144865, 0.24816390872001648, -0.17360293865203857, -0.03676899895071983, -0.17564819753170013, 0.18998438119888306, -0.050583917647600174, -0.006488069426268339, 0.10649101436138153, -0.024557121098041534, -0.103276826441288, 0.18448011577129364]], "b_1": [0.2935388386249542, 0.10967712104320526, -0.014007942751049995, 0.211833655834198, 0.33605605363845825, 0.37722209095954895, -0.16615016758441925, 0.3134673535823822, 0.06695777177810669, 0.3425212800502777, 0.3769673705101013, 0.23186539113521576, 0.5770409107208252, -0.05929069593548775, 0.01839117519557476, 0.03828774020075798], "w_2": [[-0.06261160969734192, 0.010185074992477894, -0.06083013117313385, -0.04531499370932579, -0.08979734033346176, 0.3432150185108185, -0.019801849499344826, 0.3010321259498596], [0.19698476791381836, -0.009238275699317455, 0.08842222392559052, -0.09516377002000809, -0.05022778362035751, 0.13626104593276978, -0.052890390157699585, 0.15569131076335907], [0.0724768117070198, -0.09018408507108688, 0.06850195676088333, -0.025572121143341064, 0.0680626779794693, -0.07648195326328278, 0.07993496209383011, -0.059752143919467926], [1.267876386642456, -0.05755887180566788, -0.08429178595542908, 0.021366603672504425, -0.0006479775765910745, -1.4292563199996948, -0.08077696710824966, -1.414825439453125], [0.04535430669784546, 0.06555880606174469, -0.027145234867930412, -0.07661093026399612, -0.05702832341194153, 0.23650476336479187, 0.0024587824009358883, 0.20126521587371826], [0.006042032968252897, 0.042880818247795105, 0.002187949838116765, -0.017126334831118584, -0.08352015167474747, 0.19801731407642365, -0.029196614399552345, 0.23713473975658417], [-0.01644900068640709, -0.04358499124646187, 0.014584392309188843, 0.07155826687812805, -0.09354910999536514, -0.033351872116327286, 0.07138452678918839, -0.04755295440554619], [-1.1012420654296875, -0.03534531593322754, 0.02167935110628605, -0.01116552110761404, -0.08436500281095505, 1.1038788557052612, 0.027903547510504723, 1.0676132440567017], [0.03843916580080986, -0.0952216386795044, 0.039226632565259933, 0.002778085647150874, -0.020275786519050598, -0.07848760485649109, 0.04803166165947914, 0.015538203530013561], [0.018385495990514755, -0.025189843028783798, 0.0036680365446954966, -0.02105865254998207, 0.04808586835861206, 0.1575016975402832, 0.02703506126999855, 0.23039312660694122], [-0.0033881019335240126, -0.10210853815078735, -0.04877309128642082, 0.006989633198827505, 0.046798162162303925, 0.38676899671554565, -0.032304272055625916, 0.2345031052827835], [0.22092825174331665, -0.09642873704433441, 0.04499409720301628, 0.05108088254928589, -0.10191166400909424, 0.12818090617656708, -0.021021494641900063, 0.09440375864505768], [0.1212429478764534, -0.028194155544042587, -0.0981956496834755, 0.08226924389600754, 0.055346839129924774, 0.27067816257476807, -0.09064067900180817, 0.12580905854701996], [-1.6740131378173828, -0.02066155895590782, -0.05924689769744873, 0.06347910314798355, -0.07821853458881378, 1.2807466983795166, 0.04589352011680603, 1.310766577720642], [-0.09893272817134857, -0.04093599319458008, -0.02502273954451084, 0.09490344673395157, -0.0211324505507946, -0.09021010994911194, 0.07936318963766098, -0.03593116253614426], [-0.08490308374166489, -0.015558987855911255, -0.048692114651203156, -0.007421435788273811, -0.040531404316425323, 0.25889304280281067, 0.06012800335884094, 0.27946868538856506]], "b_2": [0.07973937690258026, -0.010446485131978989, -0.003066520905122161, -0.031895797699689865, 0.006032303906977177, 0.24106740951538086, -0.008969511836767197, 0.2872662842273712], "w_3": [[-1.364486813545227, -0.11682678014039993, 0.01764785870909691, 0.03926877677440643], [-0.05695437639951706, 0.05472218990325928, 0.1266128271818161, 0.09950875490903854], [0.11415273696184158, -0.10069356113672256, 0.0864749327301979, -0.043946366757154465], [-0.10138195008039474, -0.040128443390131, -0.08937158435583115, -0.0048376512713730335], [-0.0028251828625798225, -0.04743027314543724, 0.06340016424655914, 0.07277824729681015], [0.49482327699661255, -0.06410001963376999, -0.0999293103814125, -0.14250673353672028], [0.042802367359399796, 0.0015462725423276424, -0.05991362780332565, 0.1022040992975235], [0.3523194193840027, 0.07343732565641403, 0.04157765582203865, -0.12358107417821884]], "b_3": [0.2653026282787323, -0.058485131710767746, -0.0744510293006897, 0.012550175189971924], "w_4": [[0.5988775491714478, 0.09668736904859543], [-0.04360569268465042, 0.06491032242774963], [-0.11868984252214432, -0.09601487964391708], [-0.06554870307445526, -0.14189276099205017]], "b_4": [-0.08148707449436188, -2.8251802921295166], "input_norm_mat": [[-3.0, 3.0], [-3.0, 3.0], [0.0, 40.0], [-3.0, 3.0]], "output_norm_mat": [-1.0, 1.0], "temperature": 100.0}} \ No newline at end of file +{"CHEVROLET BOLT EUV 2022": {"w_1": [[0.3452189564704895, -0.15614677965641022, -0.04062516987323761, -0.5960758328437805, 0.3211185932159424, 0.31732726097106934, -0.04430829733610153, -0.37327295541763306, -0.14118380844593048, 0.12712529301643372, 0.2641555070877075, -0.3451094627380371, -0.005127656273543835, 0.6185108423233032, 0.03725295141339302, 0.3763789236545563], [-0.0708412230014801, 0.3667356073856354, 0.031383827328681946, 0.1740853488445282, -0.04695861041545868, 0.018055908381938934, 0.009072160348296165, -0.23640218377113342, -0.10362917929887772, 0.022628149017691612, -0.224413201212883, 0.20718418061733246, -0.016947750002145767, -0.3872031271457672, -0.15500062704086304, -0.06375953555107117], [-0.0838046595454216, -0.0242826659232378, -0.07765661180019379, 0.028858814388513565, -0.09516210108995438, 0.008368706330657005, 0.1689300835132599, 0.015036891214549541, -0.15121428668498993, 0.1388195902109146, 0.11486363410949707, 0.0651545450091362, 0.13559958338737488, 0.04300367832183838, -0.13856294751167297, -0.058136988431215286], [-0.006249868310987949, 0.08809533715248108, -0.040690965950489044, 0.02359287068247795, -0.00766348373144865, 0.24816390872001648, -0.17360293865203857, -0.03676899895071983, -0.17564819753170013, 0.18998438119888306, -0.050583917647600174, -0.006488069426268339, 0.10649101436138153, -0.024557121098041534, -0.103276826441288, 0.18448011577129364]], "b_1": [0.2935388386249542, 0.10967712104320526, -0.014007942751049995, 0.211833655834198, 0.33605605363845825, 0.37722209095954895, -0.16615016758441925, 0.3134673535823822, 0.06695777177810669, 0.3425212800502777, 0.3769673705101013, 0.23186539113521576, 0.5770409107208252, -0.05929069593548775, 0.01839117519557476, 0.03828774020075798], "w_2": [[-0.06261160969734192, 0.010185074992477894, -0.06083013117313385, -0.04531499370932579, -0.08979734033346176, 0.3432150185108185, -0.019801849499344826, 0.3010321259498596], [0.19698476791381836, -0.009238275699317455, 0.08842222392559052, -0.09516377002000809, -0.05022778362035751, 0.13626104593276978, -0.052890390157699585, 0.15569131076335907], [0.0724768117070198, -0.09018408507108688, 0.06850195676088333, -0.025572121143341064, 0.0680626779794693, -0.07648195326328278, 0.07993496209383011, -0.059752143919467926], [1.267876386642456, -0.05755887180566788, -0.08429178595542908, 0.021366603672504425, -0.0006479775765910745, -1.4292563199996948, -0.08077696710824966, -1.414825439453125], [0.04535430669784546, 0.06555880606174469, -0.027145234867930412, -0.07661093026399612, -0.05702832341194153, 0.23650476336479187, 0.0024587824009358883, 0.20126521587371826], [0.006042032968252897, 0.042880818247795105, 0.002187949838116765, -0.017126334831118584, -0.08352015167474747, 0.19801731407642365, -0.029196614399552345, 0.23713473975658417], [-0.01644900068640709, -0.04358499124646187, 0.014584392309188843, 0.07155826687812805, -0.09354910999536514, -0.033351872116327286, 0.07138452678918839, -0.04755295440554619], [-1.1012420654296875, -0.03534531593322754, 0.02167935110628605, -0.01116552110761404, -0.08436500281095505, 1.1038788557052612, 0.027903547510504723, 1.0676132440567017], [0.03843916580080986, -0.0952216386795044, 0.039226632565259933, 0.002778085647150874, -0.020275786519050598, -0.07848760485649109, 0.04803166165947914, 0.015538203530013561], [0.018385495990514755, -0.025189843028783798, 0.0036680365446954966, -0.02105865254998207, 0.04808586835861206, 0.1575016975402832, 0.02703506126999855, 0.23039312660694122], [-0.0033881019335240126, -0.10210853815078735, -0.04877309128642082, 0.006989633198827505, 0.046798162162303925, 0.38676899671554565, -0.032304272055625916, 0.2345031052827835], [0.22092825174331665, -0.09642873704433441, 0.04499409720301628, 0.05108088254928589, -0.10191166400909424, 0.12818090617656708, -0.021021494641900063, 0.09440375864505768], [0.1212429478764534, -0.028194155544042587, -0.0981956496834755, 0.08226924389600754, 0.055346839129924774, 0.27067816257476807, -0.09064067900180817, 0.12580905854701996], [-1.6740131378173828, -0.02066155895590782, -0.05924689769744873, 0.06347910314798355, -0.07821853458881378, 1.2807466983795166, 0.04589352011680603, 1.310766577720642], [-0.09893272817134857, -0.04093599319458008, -0.02502273954451084, 0.09490344673395157, -0.0211324505507946, -0.09021010994911194, 0.07936318963766098, -0.03593116253614426], [-0.08490308374166489, -0.015558987855911255, -0.048692114651203156, -0.007421435788273811, -0.040531404316425323, 0.25889304280281067, 0.06012800335884094, 0.27946868538856506]], "b_2": [0.07973937690258026, -0.010446485131978989, -0.003066520905122161, -0.031895797699689865, 0.006032303906977177, 0.24106740951538086, -0.008969511836767197, 0.2872662842273712], "w_3": [[-1.364486813545227, -0.11682678014039993, 0.01764785870909691, 0.03926877677440643], [-0.05695437639951706, 0.05472218990325928, 0.1266128271818161, 0.09950875490903854], [0.11415273696184158, -0.10069356113672256, 0.0864749327301979, -0.043946366757154465], [-0.10138195008039474, -0.040128443390131, -0.08937158435583115, -0.0048376512713730335], [-0.0028251828625798225, -0.04743027314543724, 0.06340016424655914, 0.07277824729681015], [0.49482327699661255, -0.06410001963376999, -0.0999293103814125, -0.14250673353672028], [0.042802367359399796, 0.0015462725423276424, -0.05991362780332565, 0.1022040992975235], [0.3523194193840027, 0.07343732565641403, 0.04157765582203865, -0.12358107417821884]], "b_3": [0.2653026282787323, -0.058485131710767746, -0.0744510293006897, 0.012550175189971924], "w_4": [[0.5988775491714478, 0.09668736904859543], [-0.04360569268465042, 0.06491032242774963], [-0.11868984252214432, -0.09601487964391708], [-0.06554870307445526, -0.14189276099205017]], "b_4": [-0.08148707449436188, -2.8251802921295166], "input_norm_mat": [[-3.0, 3.0], [-3.0, 3.0], [0.0, 40.0], [-3.0, 3.0]], "output_norm_mat": [-1.0, 1.0], "temperature": 100.0} +,"CHEVROLET BOLT EV NO ACC": {"w_1": [[0.3452189564704895, -0.15614677965641022, -0.04062516987323761, -0.5960758328437805, 0.3211185932159424, 0.31732726097106934, -0.04430829733610153, -0.37327295541763306, -0.14118380844593048, 0.12712529301643372, 0.2641555070877075, -0.3451094627380371, -0.005127656273543835, 0.6185108423233032, 0.03725295141339302, 0.3763789236545563], [-0.0708412230014801, 0.3667356073856354, 0.031383827328681946, 0.1740853488445282, -0.04695861041545868, 0.018055908381938934, 0.009072160348296165, -0.23640218377113342, -0.10362917929887772, 0.022628149017691612, -0.224413201212883, 0.20718418061733246, -0.016947750002145767, -0.3872031271457672, -0.15500062704086304, -0.06375953555107117], [-0.0838046595454216, -0.0242826659232378, -0.07765661180019379, 0.028858814388513565, -0.09516210108995438, 0.008368706330657005, 0.1689300835132599, 0.015036891214549541, -0.15121428668498993, 0.1388195902109146, 0.11486363410949707, 0.0651545450091362, 0.13559958338737488, 0.04300367832183838, -0.13856294751167297, -0.058136988431215286], [-0.006249868310987949, 0.08809533715248108, -0.040690965950489044, 0.02359287068247795, -0.00766348373144865, 0.24816390872001648, -0.17360293865203857, -0.03676899895071983, -0.17564819753170013, 0.18998438119888306, -0.050583917647600174, -0.006488069426268339, 0.10649101436138153, -0.024557121098041534, -0.103276826441288, 0.18448011577129364]], "b_1": [0.2935388386249542, 0.10967712104320526, -0.014007942751049995, 0.211833655834198, 0.33605605363845825, 0.37722209095954895, -0.16615016758441925, 0.3134673535823822, 0.06695777177810669, 0.3425212800502777, 0.3769673705101013, 0.23186539113521576, 0.5770409107208252, -0.05929069593548775, 0.01839117519557476, 0.03828774020075798], "w_2": [[-0.06261160969734192, 0.010185074992477894, -0.06083013117313385, -0.04531499370932579, -0.08979734033346176, 0.3432150185108185, -0.019801849499344826, 0.3010321259498596], [0.19698476791381836, -0.009238275699317455, 0.08842222392559052, -0.09516377002000809, -0.05022778362035751, 0.13626104593276978, -0.052890390157699585, 0.15569131076335907], [0.0724768117070198, -0.09018408507108688, 0.06850195676088333, -0.025572121143341064, 0.0680626779794693, -0.07648195326328278, 0.07993496209383011, -0.059752143919467926], [1.267876386642456, -0.05755887180566788, -0.08429178595542908, 0.021366603672504425, -0.0006479775765910745, -1.4292563199996948, -0.08077696710824966, -1.414825439453125], [0.04535430669784546, 0.06555880606174469, -0.027145234867930412, -0.07661093026399612, -0.05702832341194153, 0.23650476336479187, 0.0024587824009358883, 0.20126521587371826], [0.006042032968252897, 0.042880818247795105, 0.002187949838116765, -0.017126334831118584, -0.08352015167474747, 0.19801731407642365, -0.029196614399552345, 0.23713473975658417], [-0.01644900068640709, -0.04358499124646187, 0.014584392309188843, 0.07155826687812805, -0.09354910999536514, -0.033351872116327286, 0.07138452678918839, -0.04755295440554619], [-1.1012420654296875, -0.03534531593322754, 0.02167935110628605, -0.01116552110761404, -0.08436500281095505, 1.1038788557052612, 0.027903547510504723, 1.0676132440567017], [0.03843916580080986, -0.0952216386795044, 0.039226632565259933, 0.002778085647150874, -0.020275786519050598, -0.07848760485649109, 0.04803166165947914, 0.015538203530013561], [0.018385495990514755, -0.025189843028783798, 0.0036680365446954966, -0.02105865254998207, 0.04808586835861206, 0.1575016975402832, 0.02703506126999855, 0.23039312660694122], [-0.0033881019335240126, -0.10210853815078735, -0.04877309128642082, 0.006989633198827505, 0.046798162162303925, 0.38676899671554565, -0.032304272055625916, 0.2345031052827835], [0.22092825174331665, -0.09642873704433441, 0.04499409720301628, 0.05108088254928589, -0.10191166400909424, 0.12818090617656708, -0.021021494641900063, 0.09440375864505768], [0.1212429478764534, -0.028194155544042587, -0.0981956496834755, 0.08226924389600754, 0.055346839129924774, 0.27067816257476807, -0.09064067900180817, 0.12580905854701996], [-1.6740131378173828, -0.02066155895590782, -0.05924689769744873, 0.06347910314798355, -0.07821853458881378, 1.2807466983795166, 0.04589352011680603, 1.310766577720642], [-0.09893272817134857, -0.04093599319458008, -0.02502273954451084, 0.09490344673395157, -0.0211324505507946, -0.09021010994911194, 0.07936318963766098, -0.03593116253614426], [-0.08490308374166489, -0.015558987855911255, -0.048692114651203156, -0.007421435788273811, -0.040531404316425323, 0.25889304280281067, 0.06012800335884094, 0.27946868538856506]], "b_2": [0.07973937690258026, -0.010446485131978989, -0.003066520905122161, -0.031895797699689865, 0.006032303906977177, 0.24106740951538086, -0.008969511836767197, 0.2872662842273712], "w_3": [[-1.364486813545227, -0.11682678014039993, 0.01764785870909691, 0.03926877677440643], [-0.05695437639951706, 0.05472218990325928, 0.1266128271818161, 0.09950875490903854], [0.11415273696184158, -0.10069356113672256, 0.0864749327301979, -0.043946366757154465], [-0.10138195008039474, -0.040128443390131, -0.08937158435583115, -0.0048376512713730335], [-0.0028251828625798225, -0.04743027314543724, 0.06340016424655914, 0.07277824729681015], [0.49482327699661255, -0.06410001963376999, -0.0999293103814125, -0.14250673353672028], [0.042802367359399796, 0.0015462725423276424, -0.05991362780332565, 0.1022040992975235], [0.3523194193840027, 0.07343732565641403, 0.04157765582203865, -0.12358107417821884]], "b_3": [0.2653026282787323, -0.058485131710767746, -0.0744510293006897, 0.012550175189971924], "w_4": [[0.5988775491714478, 0.09668736904859543], [-0.04360569268465042, 0.06491032242774963], [-0.11868984252214432, -0.09601487964391708], [-0.06554870307445526, -0.14189276099205017]], "b_4": [-0.08148707449436188, -2.8251802921295166], "input_norm_mat": [[-3.0, 3.0], [-3.0, 3.0], [0.0, 40.0], [-3.0, 3.0]], "output_norm_mat": [-1.0, 1.0], "temperature": 100.0}} diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index e3fcdc5..9092031 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -18,6 +18,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] # Tesla has high torque "TESLA AP1 MODEL S" = [nan, 2.5, nan] "TESLA AP2 MODEL S" = [nan, 2.5, nan] +"TESLA MODEL S RAVEN" = [nan, 2.5, nan] # Guess "FORD BRONCO SPORT 1ST GEN" = [nan, 1.5, nan] @@ -40,11 +41,13 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CADILLAC ESCALADE 2017" = [1.899999976158142, 1.842270016670227, 0.1120000034570694] "CADILLAC ESCALADE ESV 2019" = [1.15, 1.3, 0.2] "CADILLAC XT4 2023" = [1.45, 1.6, 0.2] +"BUICK BABY ENCLAVE 2020" = [1.45, 1.6, 0.2] "CHEVROLET BOLT EUV 2022" = [2.0, 2.0, 0.05] "CHEVROLET SILVERADO 1500 2020" = [1.9, 1.9, 0.112] "CHEVROLET TRAILBLAZER 2021" = [1.33, 1.9, 0.16] "CHEVROLET EQUINOX 2019" = [2.5, 2.5, 0.05] "CHEVROLET TRAX 2024" = [1.33, 1.9, 0.16] +"VOLKSWAGEN CADDY 3RD GEN" = [1.2, 1.2, 0.1] "VOLKSWAGEN PASSAT NMS" = [2.5, 2.5, 0.1] "VOLKSWAGEN SHARAN 2ND GEN" = [2.5, 2.5, 0.1] "HYUNDAI SANTA CRUZ 1ST GEN" = [2.7, 2.7, 0.1] diff --git a/selfdrive/car/torque_data/params.toml b/selfdrive/car/torque_data/params.toml index d703b0d..8bb89e0 100644 --- a/selfdrive/car/torque_data/params.toml +++ b/selfdrive/car/torque_data/params.toml @@ -11,8 +11,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CHRYSLER PACIFICA HYBRID 2018" = [2.08887, 1.2943025830995154, 0.114818] "CHRYSLER PACIFICA HYBRID 2019" = [1.90120, 1.1958788168371808, 0.131520] "GENESIS G70 2018" = [3.8520195946707947, 2.354697063349854, 0.06830285485626221] -"HONDA ACCORD 2018" = [1.7135052593468778, 0.3461280068322071, 0.21579936052863807] -"HONDA ACCORD HYBRID 2018" = [1.6651615004829625, 0.30322180951193245, 0.2083000440586149] +"HONDA ACCORD 2018" = [1.6893333799149202, 0.3246749081720698, 0.2120497022936265] "HONDA CIVIC (BOSCH) 2019" = [1.691708637466905, 0.40132900729454185, 0.25460295304024094] "HONDA CIVIC 2016" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544] "HONDA CLARITY 2018" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544] diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 18e6e73..92fc244 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -3,15 +3,18 @@ from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ create_gas_interceptor_command, make_can_msg +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR from opendbc.can.packer import CANPacker +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED + +LongCtrlState = car.CarControl.Actuators.LongControlState SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert -LongCtrlState = car.CarControl.Actuators.LongControlState # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -35,10 +38,21 @@ COMPENSATORY_CALCULATION_THRESHOLD_BP = [0., 11., 23.] # m/s # Lock / unlock door commands - Credit goes to AlexandreSato! LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00' UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00' + PARK = car.CarState.GearShifter.park -class CarController: +def compute_gb_toyota(accel, speed): + creep_brake = 0.0 + creep_speed = 2.3 + creep_brake_value = 0.15 + if speed < creep_speed: + creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value + gb = accel - creep_brake + return gb + + +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.params = CarControllerParams(self.CP) @@ -47,9 +61,10 @@ class CarController: self.last_angle = 0 self.alert_active = False self.last_standstill = False + self.prohibit_neg_calculation = True self.standstill_req = False self.steer_rate_counter = 0 - self.prohibit_neg_calculation = True + self.distance_button = 0 self.packer = CANPacker(dbc_name) self.gas = 0 @@ -64,6 +79,8 @@ class CarController: self.doors_locked = False self.doors_unlocked = True + self.pcm_accel_comp = 0 + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl @@ -136,34 +153,56 @@ class CarController: pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) - elif self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint in STOP_AND_GO_CAR and actuators.accel > 0.0 \ - and CS.out.standstill: - interceptor_gas_cmd = 0.12 + elif self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint in STOP_AND_GO_CAR and actuators.accel > 0.0: + interceptor_gas_cmd = 0.12 if CS.out.standstill else 0. else: interceptor_gas_cmd = 0. # prohibit negative compensatory calculations when first activating long after accelerator depression or engagement if not CC.longActive: self.prohibit_neg_calculation = True + comp_thresh = interp(CS.out.vEgo, COMPENSATORY_CALCULATION_THRESHOLD_BP, COMPENSATORY_CALCULATION_THRESHOLD_V) # don't reset until a reasonable compensatory value is reached if CS.pcm_neutral_force > comp_thresh * self.CP.mass: self.prohibit_neg_calculation = False - # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping - should_compensate = True - if (self.CP.carFingerprint in NO_STOP_TIMER_CAR and actuators.accel < 1e-3 or stopping) or CS.out.vEgo < 1e-3: - should_compensate = False + # limit minimum to only positive until first positive is reached after engagement, don't calculate when long isn't active - if CC.longActive and should_compensate and not self.prohibit_neg_calculation and (self.cydia_tune or self.frogs_go_moo_tune): + if CC.longActive and not self.prohibit_neg_calculation and (self.cydia_tune or self.frogs_go_moo_tune): accel_offset = CS.pcm_neutral_force / self.CP.mass else: accel_offset = 0. + # only calculate pcm_accel_cmd when long is active to prevent disengagement from accelerator depression if CC.longActive: if frogpilot_variables.sport_plus: - pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + if self.frogs_go_moo_tune: + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + + gas_accel = compute_gb_toyota(actuators.accel, CS.out.vEgo) + wind_brake + self.pcm_accel_comp = clip(gas_accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.03, self.pcm_accel_comp + 0.03) + pcm_accel_cmd = gas_accel + self.pcm_accel_comp + + if not CC.longActive: + pcm_accel_cmd = 0.0 + + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + else: + pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) else: - pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + if self.frogs_go_moo_tune: + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + + gas_accel = compute_gb_toyota(actuators.accel, CS.out.vEgo) + wind_brake + self.pcm_accel_comp = clip(gas_accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.03, self.pcm_accel_comp + 0.03) + pcm_accel_cmd = gas_accel + self.pcm_accel_comp + + if not CC.longActive: + pcm_accel_cmd = 0.0 + + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + else: + pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX) else: pcm_accel_cmd = 0. @@ -191,16 +230,23 @@ class CarController: # when stopping, send -2.5 raw acceleration immediately to prevent vehicle from creeping, else send actuators.accel accel_raw = -2.5 if stopping and (self.cydia_tune or self.frogs_go_moo_tune) else actuators.accel + # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup + if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: + desired_distance = 4 - hud_control.leadDistanceBars + if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance: + self.distance_button = not self.distance_button + else: + self.distance_button = 0 + # Lexus IS uses a different cancellation message if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, accel_raw, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, - CS.distance_button, frogpilot_variables)) + self.distance_button, frogpilot_variables)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, - CS.distance_button, frogpilot_variables)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, frogpilot_variables)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. @@ -247,15 +293,17 @@ class CarController: new_actuators.gas = self.gas # Lock doors when in drive / unlock doors when in park - if frogpilot_variables.lock_doors: - if self.doors_unlocked and CS.out.gearShifter != PARK: + if self.doors_unlocked and CS.out.gearShifter != PARK and CS.out.vEgo >= CRUISING_SPEED: + if frogpilot_variables.lock_doors: can_sends.append(make_can_msg(0x750, LOCK_CMD, 0)) - self.doors_locked = True - self.doors_unlocked = False - elif self.doors_locked and CS.out.gearShifter == PARK: + self.doors_locked = True + self.doors_unlocked = False + + elif self.doors_locked and CS.out.gearShifter == PARK: + if frogpilot_variables.unlock_doors: can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0)) - self.doors_locked = False - self.doors_unlocked = True + self.doors_locked = False + self.doors_unlocked = True self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 52cff5f..9150156 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -10,9 +10,6 @@ from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR -from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS - -from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController SteerControlType = car.CarParams.SteerControlType @@ -45,19 +42,38 @@ class CarState(CarStateBase): self.accurate_steer_angle_seen = False self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) + self.prev_distance_button = 0 + self.distance_button = 0 + + self.pcm_follow_distance = 0 + self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} # FrogPilot variables - self.profile_restored = False self.zss_compute = False self.zss_cruise_active_last = False + self.pcm_accel_net = 0.0 + self.pcm_neutral_force = 0.0 self.zss_angle_offset = 0 self.zss_threshold_count = 0 - self.traffic_signals = {} + # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! + def calculate_speed_limit(self, cp_cam, frogpilot_variables): + signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] + traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} + + tsgn1 = traffic_signals.get("TSGN1", None) + spdval1 = traffic_signals.get("SPDVAL1", None) + + if tsgn1 == 1 and not frogpilot_variables.force_mph_dashboard: + return spdval1 * CV.KPH_TO_MS + elif tsgn1 == 36 or frogpilot_variables.force_mph_dashboard: + return spdval1 * CV.MPH_TO_MS + else: + return 0 def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() @@ -182,88 +198,37 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) - # FrogPilot functions - - # Switch the current state of Experimental Mode if the LKAS button is double pressed - if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V: - message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] - lkas_pressed = any(self.lkas_hud.get(key) == 1 for key in message_keys) - - if lkas_pressed and not self.lkas_previously_pressed: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_lkas() - else: - self.fpf.update_experimental_mode() - - self.lkas_previously_pressed = lkas_pressed - if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: - # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2" - self.personality_profile = cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1 + self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] - if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or self.CP.flags & ToyotaFlags.SMART_DSU: + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): # distance button is wired to the ACC module (camera or radar) + self.prev_distance_button = self.distance_button if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): - distance_pressed = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] else: - distance_pressed = cp.vl["SDSU"]["FD_BUTTON"] - else: - distance_pressed = False + self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] - # Distance button functions - if ret.cruiseState.available: - if distance_pressed: - self.distance_pressed_counter += 1 - elif self.distance_previously_pressed: - # Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds - if self.distance_pressed_counter < CRUISE_LONG_PRESS: - self.previous_personality_profile = (self.personality_profile + 2) % 3 - self.fpf.distance_button_function(self.previous_personality_profile) - self.profile_restored = False - self.distance_pressed_counter = 0 + if self.CP.carFingerprint != CAR.PRIUS_V: + self.lkas_previously_enabled = self.lkas_enabled + message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] + self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys) - # Switch the current state of Experimental Mode if the button is held down for 0.5 seconds - if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() + self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp_cam, frogpilot_variables)) - # Switch the current state of Traffic Mode if the button is held down for 2.5 seconds - if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: - self.fpf.update_traffic_mode() + self.cruise_decreased_previously = self.cruise_decreased + self.cruise_increased_previously = self.cruise_increased - # Revert the previous changes to Experimental Mode - if frogpilot_variables.experimental_mode_via_distance: - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() + self.cruise_decreased = self.pcm_acc_status == 10 + self.cruise_increased = self.pcm_acc_status == 9 - self.distance_previously_pressed = distance_pressed - - # Update the distance lines on the dash upon ignition/onroad UI button clicked - if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: - # Sync with the onroad UI button - if self.fpf.personality_changed_via_ui: - self.profile_restored = False - self.previous_personality_profile = self.fpf.current_personality - self.fpf.reset_personality_changed_param() - - # Set personality to the previous drive's personality or when the user changes it via the UI - if self.personality_profile == self.previous_personality_profile: - self.profile_restored = True - if not self.profile_restored: - self.distance_button = not self.distance_button - - # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! - self.update_traffic_signals(cp_cam) - SpeedLimitController.car_speed_limit = self.calculate_speed_limit(frogpilot_variables) - SpeedLimitController.write_car_state() + self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"] + self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] # ZSS Support - Credit goes to the DragonPilot team! if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT: zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"] + # Only compute ZSS offset when acc is active zss_cruise_active = ret.cruiseState.available if zss_cruise_active and not self.zss_cruise_active_last: @@ -287,24 +252,6 @@ class CarState(CarStateBase): return ret - def update_traffic_signals(self, cp_cam): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - new_values = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - if new_values != self.traffic_signals: - self.traffic_signals.update(new_values) - - def calculate_speed_limit(self, frogpilot_variables): - tsgn1 = self.traffic_signals.get("TSGN1", None) - spdval1 = self.traffic_signals.get("SPDVAL1", None) - - if tsgn1 == 1 and not frogpilot_variables.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_variables.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - else: - return 0 - @staticmethod def get_can_parser(CP): messages = [ @@ -353,7 +300,7 @@ class CarState(CarStateBase): ("PRE_COLLISION", 33), ] - if CP.flags & ToyotaFlags.SMART_DSU: + if CP.flags & ToyotaFlags.SMART_DSU and not CP.flags & ToyotaFlags.RADAR_CAN_FILTER: messages += [ ("SDSU", 100), ] diff --git a/selfdrive/car/toyota/fingerprints.py b/selfdrive/car/toyota/fingerprints.py index 12a1d46..a0d1ef2 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/selfdrive/car/toyota/fingerprints.py @@ -573,6 +573,7 @@ FW_VERSIONS = { b'\x018821F6201400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F12010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -638,6 +639,7 @@ FW_VERSIONS = { (Ecu.dsu, 0x791, None): [ b'881510E01100\x00\x00\x00\x00', b'881510E01200\x00\x00\x00\x00', + b'881510E02200\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702100\x00\x00\x00\x00', @@ -685,6 +687,7 @@ FW_VERSIONS = { b'\x01896630EB1000\x00\x00\x00\x00', b'\x01896630EB1100\x00\x00\x00\x00', b'\x01896630EB1200\x00\x00\x00\x00', + b'\x01896630EB1300\x00\x00\x00\x00', b'\x01896630EB2000\x00\x00\x00\x00', b'\x01896630EB2100\x00\x00\x00\x00', b'\x01896630EB2200\x00\x00\x00\x00', @@ -771,16 +774,22 @@ FW_VERSIONS = { b'\x018966353S1000\x00\x00\x00\x00', b'\x018966353S2000\x00\x00\x00\x00', ], + (Ecu.engine, 0x7e0, None): [ + b'\x02353U0000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00', + ], (Ecu.abs, 0x7b0, None): [ b'\x01F15265337200\x00\x00\x00\x00', b'\x01F15265342000\x00\x00\x00\x00', + b'\x01F15265343000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B53450\x00\x00\x00\x00\x00\x00', + b'8965B53800\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', b'\x018821F6201300\x00\x00\x00\x00', + b'\x018821F6201400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F5303300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', @@ -843,6 +852,7 @@ FW_VERSIONS = { b'8965B47023\x00\x00\x00\x00\x00\x00', b'8965B47050\x00\x00\x00\x00\x00\x00', b'8965B47060\x00\x00\x00\x00\x00\x00', + b'8965B47070\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152647290\x00\x00\x00\x00\x00\x00', @@ -1024,6 +1034,7 @@ FW_VERSIONS = { b'\x02896634A13000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A13101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896634A13201\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -1132,6 +1143,7 @@ FW_VERSIONS = { b'\x01F15264283300\x00\x00\x00\x00', b'\x01F152642F1000\x00\x00\x00\x00', b'\x01F152642F8000\x00\x00\x00\x00', + b'\x01F152642F8100\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00', @@ -1144,6 +1156,7 @@ FW_VERSIONS = { b'\x01896634AF0000\x00\x00\x00\x00', b'\x01896634AJ2000\x00\x00\x00\x00', b'\x01896634AL5000\x00\x00\x00\x00', + b'\x01896634AL6000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R03100\x00\x00\x00\x00', @@ -1263,6 +1276,7 @@ FW_VERSIONS = { }, CAR.LEXUS_ES: { (Ecu.engine, 0x7e0, None): [ + b'\x02333M4100\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02333R0000\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -1533,6 +1547,7 @@ FW_VERSIONS = { b'\x018966348W5100\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', b'\x018966348X0000\x00\x00\x00\x00', + b'\x01896634D11000\x00\x00\x00\x00', b'\x01896634D12000\x00\x00\x00\x00', b'\x01896634D12100\x00\x00\x00\x00', b'\x01896634D43000\x00\x00\x00\x00', diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 1da33f2..2f5ec79 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,15 +1,16 @@ -from cereal import car -from openpilot.common.conversions import Conversions as CV +from cereal import car, custom from panda import Panda from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, STOP_AND_GO_CAR -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName SteerControlType = car.CarParams.SteerControlType +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -21,7 +22,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -48,8 +49,6 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - stop_and_go = candidate in TSS2_CAR - # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): @@ -58,74 +57,22 @@ class CarInterface(CarInterfaceBase): if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR: ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value + # In TSS2 cars, the camera does long control + found_ecus = [fw.ecu for fw in car_fw] + ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ + and not (ret.flags & ToyotaFlags.SMART_DSU) + if candidate == CAR.PRIUS: - ret.wheelbase = 2.70 - ret.steerRatio = 15.74 # unknown end-to-end spec - ret.tireStiffnessFactor = 0.6371 # hand-tune - ret.mass = 3045. * CV.LB_TO_KG # Only give steer angle deadzone to for bad angle sensor prius for fw in car_fw: if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': ret.steerActuatorDelay = 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2) - elif candidate == CAR.PRIUS_V: - ret.wheelbase = 2.78 - ret.steerRatio = 17.4 - ret.tireStiffnessFactor = 0.5533 - ret.mass = 3340. * CV.LB_TO_KG - - elif candidate in (CAR.RAV4, CAR.RAV4H): - ret.wheelbase = 2.65 - ret.steerRatio = 16.88 # 14.5 is spec end-to-end - ret.tireStiffnessFactor = 0.5533 - ret.mass = 3650. * CV.LB_TO_KG # mean between normal and hybrid - - elif candidate == CAR.COROLLA: - ret.wheelbase = 2.70 - ret.steerRatio = 18.27 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 2860. * CV.LB_TO_KG # mean between normal and hybrid - elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): - ret.wheelbase = 2.79 - ret.steerRatio = 16. # 14.8 is spec end-to-end ret.wheelSpeedFactor = 1.035 - ret.tireStiffnessFactor = 0.5533 - ret.mass = 4481. * CV.LB_TO_KG # mean between min and max - - elif candidate in (CAR.CHR, CAR.CHR_TSS2): - ret.wheelbase = 2.63906 - ret.steerRatio = 13.6 - ret.tireStiffnessFactor = 0.7933 - ret.mass = 3300. * CV.LB_TO_KG - - elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2): - ret.wheelbase = 2.82448 - ret.steerRatio = 13.7 - ret.tireStiffnessFactor = 0.7933 - ret.mass = 3400. * CV.LB_TO_KG # mean between normal and hybrid - - elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDER_TSS2): - # TODO: TSS-P models can do stop and go, but unclear if it requires sDSU or unplugging DSU - ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in - ret.steerRatio = 16.0 - ret.tireStiffnessFactor = 0.8 - ret.mass = 4516. * CV.LB_TO_KG # mean between normal and hybrid - - elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALON_TSS2): - # starting from 2019, all Avalon variants have stop and go - # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf - ret.wheelbase = 2.82 - ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download - ret.tireStiffnessFactor = 0.7983 - ret.mass = 3505. * CV.LB_TO_KG # mean between normal and hybrid elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023): - ret.wheelbase = 2.68986 - ret.steerRatio = 14.3 - ret.tireStiffnessFactor = 0.7933 - ret.mass = 3585. * CV.LB_TO_KG # Average between ICE and Hybrid ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP = [0.0] ret.lateralTuning.pid.kpBP = [0.0] @@ -142,92 +89,16 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00004 break - elif candidate == CAR.COROLLA_TSS2: - ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback - ret.steerRatio = 13.9 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 3060. * CV.LB_TO_KG - - elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ES_TSS2): - ret.wheelbase = 2.8702 - ret.steerRatio = 16.0 # not optimized - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 3677. * CV.LB_TO_KG # mean between min and max - - elif candidate == CAR.SIENNA: - ret.wheelbase = 3.03 - ret.steerRatio = 15.5 - ret.tireStiffnessFactor = 0.444 - ret.mass = 4590. * CV.LB_TO_KG - - elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC): - ret.wheelbase = 2.79908 - ret.steerRatio = 13.3 - ret.tireStiffnessFactor = 0.444 - ret.mass = 3736.8 * CV.LB_TO_KG - - elif candidate == CAR.LEXUS_GS_F: - ret.wheelbase = 2.84988 - ret.steerRatio = 13.3 - ret.tireStiffnessFactor = 0.444 - ret.mass = 4034. * CV.LB_TO_KG - - elif candidate == CAR.LEXUS_CTH: - ret.wheelbase = 2.60 - ret.steerRatio = 18.6 - ret.tireStiffnessFactor = 0.517 - ret.mass = 3108 * CV.LB_TO_KG # mean between min and max - - elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2): - ret.wheelbase = 2.66 - ret.steerRatio = 14.7 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 4070 * CV.LB_TO_KG - - elif candidate == CAR.LEXUS_LC_TSS2: - ret.wheelbase = 2.87 - ret.steerRatio = 13.0 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 4500 * CV.LB_TO_KG - - elif candidate == CAR.PRIUS_TSS2: - ret.wheelbase = 2.70002 # from toyota online sepc. - ret.steerRatio = 13.4 # True steerRatio from older prius - ret.tireStiffnessFactor = 0.6371 # hand-tune - ret.mass = 3115. * CV.LB_TO_KG - - elif candidate == CAR.MIRAI: - ret.wheelbase = 2.91 - ret.steerRatio = 14.8 - ret.tireStiffnessFactor = 0.8 - ret.mass = 4300. * CV.LB_TO_KG - - elif candidate == CAR.ALPHARD_TSS2: - ret.wheelbase = 3.00 - ret.steerRatio = 14.2 - ret.tireStiffnessFactor = 0.444 - ret.mass = 4305. * CV.LB_TO_KG - ret.centerToFront = ret.wheelbase * 0.44 # TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction. # Detect flipped signals and enable for C-HR and others ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR - # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it - # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs - if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): - ret.flags |= ToyotaFlags.SMART_DSU.value - # No radar dbc for cars without DSU which are not TSS 2.0 # TODO: make an adas dbc file for dsu-less models ret.radarUnavailable = DBC[candidate]['radar'] is None or candidate in (NO_DSU_CAR - TSS2_CAR) - # In TSS2 cars, the camera does long control - found_ecus = [fw.ecu for fw in car_fw] - ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ - and not (ret.flags & ToyotaFlags.SMART_DSU) - # if the smartDSU is detected, openpilot can send ACC_CONTROL and the smartDSU will block it from the DSU or radar. # since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU) @@ -250,7 +121,7 @@ class CarInterface(CarInterfaceBase): # - TSS2 radar ACC cars w/o smartDSU installed (disables radar) # - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet) ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) - ret.openpilotLongitudinalControl &= not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl &= not disable_openpilot_long ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl @@ -271,35 +142,30 @@ class CarInterface(CarInterfaceBase): # on stock Toyota this is -2.5 ret.stopAccel = -2.5 tune.deadzoneBP = [0., 16., 20., 30.] - tune.deadzoneV = [0., .03, .06, .15] - ret.stoppingDecelRate = 0.17 # This is okay for TSS-P - if candidate in TSS2_CAR: - ret.vEgoStopping = 0.25 - ret.vEgoStarting = 0.25 - ret.stoppingDecelRate = 0.009 # reach stopping target smoothly + tune.deadzoneV = [.04, .05, .08, .15] + ret.stoppingDecelRate = 0.17 tune.kpBP = [0., 5.] tune.kpV = [0.8, 1.] tune.kiBP = [0., 5.] tune.kiV = [0.3, 1.] elif params.get_bool("FrogsGoMooTune"): + # on stock Toyota this is -2.5 + ret.stopAccel = -2.5 + ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + tune.deadzoneBP = [0., 16., 20., 30.] - tune.deadzoneV = [0., .03, .06, .15] - tune.kpBP = [0., 5., 20.] - tune.kpV = [1.3, 1.0, 0.7] + tune.deadzoneV = [0., .03, .06, .15] # In MPH = [ 0, 27, 45, 60, 89] tune.kiBP = [ 0., 12., 20., 27., 40.] tune.kiV = [.35, .215, .195, .10, .01] - if candidate in TSS2_CAR: - ret.stopAccel = -2.5 - ret.stoppingDecelRate = 0.009 # reach stopping target smoothly - else: - ret.stopAccel = -2.5 # on stock Toyota this is -2.5 - ret.stoppingDecelRate = 0.3 # This is okay for TSS-P + # In MPH = [ 0, 11, 45] + tune.kpBP = [0., 5., 20.] + tune.kpV = [1.3, 1.0, 0.7] - ret.vEgoStarting = 0.1 - ret.vEgoStopping = 0.1 + ret.vEgoStopping = 0.15 # car is near 0.1 to 0.2 when car starts requesting stopping accel + ret.vEgoStarting = 0.15 # needs to be > or == vEgoStopping elif (candidate in TSS2_CAR or ret.enableGasInterceptor) and params.get_bool("DragonPilotTune"): # Credit goes to the DragonPilot team! tune.deadzoneBP = [0., 16., 20., 30.] @@ -342,8 +208,16 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}), + *create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}), + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + # events - events = self.create_common_events(ret, frogpilot_variables) + events = self.create_common_events(ret) # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until # the more accurate angle sensor signal is initialized diff --git a/selfdrive/car/toyota/tests/__init__.py b/selfdrive/car/toyota/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/car/toyota/tests/print_platform_codes.py b/selfdrive/car/toyota/tests/print_platform_codes.py new file mode 100644 index 0000000..9ec7a14 --- /dev/null +++ b/selfdrive/car/toyota/tests/print_platform_codes.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +from collections import defaultdict +from cereal import car +from openpilot.selfdrive.car.toyota.values import PLATFORM_CODE_ECUS, get_platform_codes +from openpilot.selfdrive.car.toyota.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +if __name__ == "__main__": + parts_for_ecu: dict = defaultdict(set) + cars_for_code: dict = defaultdict(lambda: defaultdict(set)) + for car_model, ecus in FW_VERSIONS.items(): + print() + print(car_model) + for ecu in sorted(ecus, key=lambda x: int(x[0])): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + platform_codes = get_platform_codes(ecus[ecu]) + parts_for_ecu[ecu] |= {code.split(b'-')[0] for code in platform_codes if code.count(b'-') > 1} + for code in platform_codes: + cars_for_code[ecu][b'-'.join(code.split(b'-')[:2])] |= {car_model} + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + print(f' Codes: {platform_codes}') + + print('\nECU parts:') + for ecu, parts in parts_for_ecu.items(): + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}): {parts}') + + print('\nCar models vs. platform codes (no major versions):') + for ecu, codes in cars_for_code.items(): + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + for code, cars in codes.items(): + print(f' {code!r}: {sorted(cars)}') diff --git a/selfdrive/car/toyota/tests/test_toyota.py b/selfdrive/car/toyota/tests/test_toyota.py new file mode 100644 index 0000000..6a2476d --- /dev/null +++ b/selfdrive/car/toyota/tests/test_toyota.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +from hypothesis import given, settings, strategies as st +import unittest + +from cereal import car +from openpilot.selfdrive.car.fw_versions import build_fw_dict +from openpilot.selfdrive.car.toyota.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, \ + FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ + get_platform_codes + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + + +def check_fw_version(fw_version: bytes) -> bool: + return b'?' not in fw_version + + +class TestToyotaInterfaces(unittest.TestCase): + def test_car_sets(self): + self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0) + self.assertTrue(len(RADAR_ACC_CAR - TSS2_CAR) == 0) + + def test_lta_platforms(self): + # At this time, only RAV4 2023 is expected to use LTA/angle control + self.assertEqual(ANGLE_CONTROL_CAR, {CAR.RAV4_TSS2_2023}) + + def test_tss2_dbc(self): + # We make some assumptions about TSS2 platforms, + # like looking up certain signals only in this DBC + for car_model, dbc in DBC.items(): + if car_model in TSS2_CAR: + self.assertEqual(dbc["pt"], "toyota_nodsu_pt_generated") + + def test_essential_ecus(self): + # Asserts standard ECUs exist for each platform + common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera} + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + present_ecus = {ecu[0] for ecu in ecus} + missing_ecus = common_ecus - present_ecus + self.assertEqual(len(missing_ecus), 0) + + # Some exceptions for other common ECUs + if car_model not in (CAR.ALPHARD_TSS2,): + self.assertIn(Ecu.abs, present_ecus) + + if car_model not in (CAR.MIRAI,): + self.assertIn(Ecu.engine, present_ecus) + + if car_model not in (CAR.PRIUS_V, CAR.LEXUS_CTH): + self.assertIn(Ecu.eps, present_ecus) + + +class TestToyotaFingerprint(unittest.TestCase): + def test_non_essential_ecus(self): + # Ensures only the cars that have multiple engine ECUs are in the engine non-essential ECU list + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + engine_ecus = {ecu for ecu in ecus if ecu[0] == Ecu.engine} + self.assertEqual(len(engine_ecus) > 1, + car_model in FW_QUERY_CONFIG.non_essential_ecus[Ecu.engine], + f"Car model unexpectedly {'not ' if len(engine_ecus) > 1 else ''}in non-essential list") + + def test_valid_fw_versions(self): + # Asserts all FW versions are valid + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for fws in ecus.values(): + for fw in fws: + self.assertTrue(check_fw_version(fw), fw) + + # Tests for part numbers, platform codes, and sub-versions which Toyota will use to fuzzy + # fingerprint in the absence of full FW matches: + @settings(max_examples=100) + @given(data=st.data()) + def test_platform_codes_fuzzy_fw(self, data): + fw_strategy = st.lists(st.binary()) + fws = data.draw(fw_strategy) + get_platform_codes(fws) + + def test_platform_code_ecus_available(self): + # Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for platform_code_ecu in PLATFORM_CODE_ECUS: + if platform_code_ecu == Ecu.eps and car_model in (CAR.PRIUS_V, CAR.LEXUS_CTH,): + continue + if platform_code_ecu == Ecu.abs and car_model in (CAR.ALPHARD_TSS2,): + continue + self.assertIn(platform_code_ecu, [e[0] for e in ecus]) + + def test_fw_format(self): + # Asserts: + # - every supported ECU FW version returns one platform code + # - every supported ECU FW version has a part number + # - expected parsing of ECU sub-versions + + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model.value): + for ecu, fws in ecus.items(): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + codes = dict() + for fw in fws: + result = get_platform_codes([fw]) + # Check only one platform code and sub-version + self.assertEqual(1, len(result), f"Unable to parse FW: {fw}") + self.assertEqual(1, len(list(result.values())[0]), f"Unable to parse FW: {fw}") + codes |= result + + # Toyota places the ECU part number in their FW versions, assert all parsable + # Note that there is only one unique part number per ECU across the fleet, so this + # is not important for identification, just a sanity check. + self.assertTrue(all(code.count(b"-") > 1 for code in codes), + f"FW does not have part number: {fw} {codes}") + + def test_platform_codes_spot_check(self): + # Asserts basic platform code parsing behavior for a few cases + results = get_platform_codes([ + b"F152607140\x00\x00\x00\x00\x00\x00", + b"F152607171\x00\x00\x00\x00\x00\x00", + b"F152607110\x00\x00\x00\x00\x00\x00", + b"F152607180\x00\x00\x00\x00\x00\x00", + ]) + self.assertEqual(results, {b"F1526-07-1": {b"10", b"40", b"71", b"80"}}) + + results = get_platform_codes([ + b"\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00", + b"\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00", + ]) + self.assertEqual(results, {b"8646F-41-04": {b"100"}}) + + # Short version has no part number + results = get_platform_codes([ + b"\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00", + b"\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00", + ]) + self.assertEqual(results, {b"58-70": {b"000"}, b"58-83": {b"000"}}) + + results = get_platform_codes([ + b"F152607110\x00\x00\x00\x00\x00\x00", + b"F152607140\x00\x00\x00\x00\x00\x00", + b"\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00", + b"\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00", + ]) + self.assertEqual(results, {b"F1526-07-1": {b"10", b"40"}, b"8646F-41-04": {b"100"}, b"58-79": {b"000"}}) + + def test_fuzzy_excluded_platforms(self): + # Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared. + platforms_with_shared_codes = set() + for platform, fw_by_addr in FW_VERSIONS.items(): + car_fw = [] + for ecu, fw_versions in fw_by_addr.items(): + ecu_name, addr, sub_addr = ecu + for fw in fw_versions: + car_fw.append({"ecu": ecu_name, "fwVersion": fw, "address": addr, + "subAddress": 0 if sub_addr is None else sub_addr}) + + CP = car.CarParams.new_message(carFw=car_fw) + matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), FW_VERSIONS) + if len(matches) == 1: + self.assertEqual(list(matches)[0], platform) + else: + # If a platform has multiple matches, add it and its matches + platforms_with_shared_codes |= {str(platform), *matches} + + self.assertEqual(platforms_with_shared_codes, FUZZY_EXCLUDED_PLATFORMS, (len(platforms_with_shared_codes), len(FW_VERSIONS))) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 37a24ef..9544c6f 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,19 +33,19 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance_button, frogpilot_variables): +def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance, frogpilot_variables): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, # compensated accel command "ACC_TYPE": acc_type, - "DISTANCE": distance_button, + "DISTANCE": distance, "MINI_CAR": lead, "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, "CANCEL_REQ": pcm_cancel, "ALLOW_LONG_PRESS": 2 if frogpilot_variables.reverse_cruise_increase else 1, "ACC_CUT_IN": fcw_alert, # only shown when ACC enabled - "ACCEL_CMD_ALT": accel_raw, # raw accel command, pcm uses this to calculate a compensatory force + "ACCEL_CMD_ALT": accel_raw, # raw accel command, pcm uses this to calculate a compensatory force } return packer.make_can_msg("ACC_CONTROL", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f91dba6..442465c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,13 +1,13 @@ import re from collections import defaultdict from dataclasses import dataclass, field -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Set, Union +from enum import Enum, IntFlag from cereal import car from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, CarParts, CarHarness +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarDocs, Column, CarParts, CarHarness from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -43,52 +43,24 @@ class CarControllerParams: class ToyotaFlags(IntFlag): + # Detected flags HYBRID = 1 SMART_DSU = 2 DISABLE_RADAR = 4 - ZSS = 8 + RADAR_CAN_FILTER = 1024 + # Static flags + TSS2 = 8 + NO_DSU = 16 + UNSUPPORTED_DSU = 32 + RADAR_ACC = 64 + # these cars use the Lane Tracing Assist (LTA) message for lateral control + ANGLE_CONTROL = 128 + NO_STOP_TIMER = 256 + # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU + SNG_WITHOUT_DSU = 512 -class CAR(StrEnum): - # Toyota - ALPHARD_TSS2 = "TOYOTA ALPHARD 2020" - AVALON = "TOYOTA AVALON 2016" - AVALON_2019 = "TOYOTA AVALON 2019" - AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5 - CAMRY = "TOYOTA CAMRY 2018" - CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 - CHR = "TOYOTA C-HR 2018" - CHR_TSS2 = "TOYOTA C-HR 2021" - COROLLA = "TOYOTA COROLLA 2017" - # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid - COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" - HIGHLANDER = "TOYOTA HIGHLANDER 2017" - HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" - PRIUS = "TOYOTA PRIUS 2017" - PRIUS_V = "TOYOTA PRIUS v 2017" - PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" - RAV4 = "TOYOTA RAV4 2017" - RAV4H = "TOYOTA RAV4 HYBRID 2017" - RAV4_TSS2 = "TOYOTA RAV4 2019" - RAV4_TSS2_2022 = "TOYOTA RAV4 2022" - RAV4_TSS2_2023 = "TOYOTA RAV4 2023" - MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 - SIENNA = "TOYOTA SIENNA 2018" - - # Lexus - LEXUS_CTH = "LEXUS CT HYBRID 2018" - LEXUS_ES = "LEXUS ES 2018" - LEXUS_ES_TSS2 = "LEXUS ES 2019" - LEXUS_IS = "LEXUS IS 2018" - LEXUS_IS_TSS2 = "LEXUS IS 2023" - LEXUS_NX = "LEXUS NX 2018" - LEXUS_NX_TSS2 = "LEXUS NX 2020" - LEXUS_LC_TSS2 = "LEXUS LC 2024" - LEXUS_RC = "LEXUS RC 2020" - LEXUS_RX = "LEXUS RX 2016" - LEXUS_RX_TSS2 = "LEXUS RX 2020" - LEXUS_GS_F = "LEXUS GS F 2016" - + ZSS = 1024 class Footnote(Enum): CAMRY = CarFootnote( @@ -97,132 +69,310 @@ class Footnote(Enum): @dataclass -class ToyotaCarInfo(CarInfo): +class ToyotaCarDocs(CarDocs): package: str = "All" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.toyota_a])) -CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { +@dataclass +class ToyotaTSS2PlatformConfig(PlatformConfig): + dbc_dict: dict = field(default_factory=lambda: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas')) + + def init(self): + self.flags |= ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU + + if self.flags & ToyotaFlags.RADAR_ACC: + self.dbc_dict = dbc_dict('toyota_nodsu_pt_generated', None) + + +class CAR(Platforms): # Toyota - CAR.ALPHARD_TSS2: [ - ToyotaCarInfo("Toyota Alphard 2019-20"), - ToyotaCarInfo("Toyota Alphard Hybrid 2021"), - ], - CAR.AVALON: [ - ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"), - ToyotaCarInfo("Toyota Avalon 2017-18"), - ], - CAR.AVALON_2019: [ - ToyotaCarInfo("Toyota Avalon 2019-21"), - ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"), - ], - CAR.AVALON_TSS2: [ - ToyotaCarInfo("Toyota Avalon 2022"), - ToyotaCarInfo("Toyota Avalon Hybrid 2022"), - ], - CAR.CAMRY: [ - ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), - ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), - ], - CAR.CAMRY_TSS2: [ - ToyotaCarInfo("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]), - ToyotaCarInfo("Toyota Camry Hybrid 2021-24"), - ], - CAR.CHR: [ - ToyotaCarInfo("Toyota C-HR 2017-20"), - ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"), - ], - CAR.CHR_TSS2: [ - ToyotaCarInfo("Toyota C-HR 2021"), - ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"), - ], - CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), - CAR.COROLLA_TSS2: [ - ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), - ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5), - ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), - # Hybrid platforms - ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"), - ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5), - ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), - ToyotaCarInfo("Lexus UX Hybrid 2019-23"), - ], - CAR.HIGHLANDER: [ - ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"), - ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"), - ], - CAR.HIGHLANDER_TSS2: [ - ToyotaCarInfo("Toyota Highlander 2020-23"), - ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"), - ], - CAR.PRIUS: [ - ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), - ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), - ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), - ], - CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED), - CAR.PRIUS_TSS2: [ - ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), - ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), - ], - CAR.RAV4: [ - ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P"), - ToyotaCarInfo("Toyota RAV4 2017-18") - ], - CAR.RAV4H: [ - ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26") - ], - CAR.RAV4_TSS2: [ - ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), - ], - CAR.RAV4_TSS2_2022: [ - ToyotaCarInfo("Toyota RAV4 2022"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), - ], - CAR.RAV4_TSS2_2023: [ - ToyotaCarInfo("Toyota RAV4 2023-24"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2023-24"), - ], - CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), - CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED), + ALPHARD_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA ALPHARD 2020", + [ + ToyotaCarDocs("Toyota Alphard 2019-20"), + ToyotaCarDocs("Toyota Alphard Hybrid 2021"), + ], + CarSpecs(mass=4305. * CV.LB_TO_KG, wheelbase=3.0, steerRatio=14.2, tireStiffnessFactor=0.444), + ) + AVALON = PlatformConfig( + "TOYOTA AVALON 2016", + [ + ToyotaCarDocs("Toyota Avalon 2016", "Toyota Safety Sense P"), + ToyotaCarDocs("Toyota Avalon 2017-18"), + ], + CarSpecs(mass=3505. * CV.LB_TO_KG, wheelbase=2.82, steerRatio=14.8, tireStiffnessFactor=0.7983), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + ) + AVALON_2019 = PlatformConfig( + "TOYOTA AVALON 2019", + [ + ToyotaCarDocs("Toyota Avalon 2019-21"), + ToyotaCarDocs("Toyota Avalon Hybrid 2019-21"), + ], + AVALON.specs, + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + ) + AVALON_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA AVALON 2022", # TSS 2.5 + [ + ToyotaCarDocs("Toyota Avalon 2022"), + ToyotaCarDocs("Toyota Avalon Hybrid 2022"), + ], + AVALON.specs, + ) + CAMRY = PlatformConfig( + "TOYOTA CAMRY 2018", + [ + ToyotaCarDocs("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), + ToyotaCarDocs("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), + ], + CarSpecs(mass=3400. * CV.LB_TO_KG, wheelbase=2.82448, steerRatio=13.7, tireStiffnessFactor=0.7933), + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_DSU, + ) + CAMRY_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA CAMRY 2021", # TSS 2.5 + [ + ToyotaCarDocs("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]), + ToyotaCarDocs("Toyota Camry Hybrid 2021-24"), + ], + CAMRY.specs, + ) + CHR = PlatformConfig( + "TOYOTA C-HR 2018", + [ + ToyotaCarDocs("Toyota C-HR 2017-20"), + ToyotaCarDocs("Toyota C-HR Hybrid 2017-20"), + ], + CarSpecs(mass=3300. * CV.LB_TO_KG, wheelbase=2.63906, steerRatio=13.6, tireStiffnessFactor=0.7933), + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_DSU, + ) + CHR_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA C-HR 2021", + [ + ToyotaCarDocs("Toyota C-HR 2021"), + ToyotaCarDocs("Toyota C-HR Hybrid 2021-22"), + ], + CHR.specs, + flags=ToyotaFlags.RADAR_ACC, + ) + COROLLA = PlatformConfig( + "TOYOTA COROLLA 2017", + [ToyotaCarDocs("Toyota Corolla 2017-19")], + CarSpecs(mass=2860. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=18.27, tireStiffnessFactor=0.444), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid + COROLLA_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA COROLLA TSS2 2019", + [ + ToyotaCarDocs("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), + ToyotaCarDocs("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5), + ToyotaCarDocs("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), + # Hybrid platforms + ToyotaCarDocs("Toyota Corolla Hybrid 2020-22"), + ToyotaCarDocs("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5), + ToyotaCarDocs("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), + ToyotaCarDocs("Lexus UX Hybrid 2019-23"), + ], + CarSpecs(mass=3060. * CV.LB_TO_KG, wheelbase=2.67, steerRatio=13.9, tireStiffnessFactor=0.444), + ) + HIGHLANDER = PlatformConfig( + "TOYOTA HIGHLANDER 2017", + [ + ToyotaCarDocs("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"), + ToyotaCarDocs("Toyota Highlander Hybrid 2017-19"), + ], + CarSpecs(mass=4516. * CV.LB_TO_KG, wheelbase=2.8194, steerRatio=16.0, tireStiffnessFactor=0.8), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU, + ) + HIGHLANDER_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA HIGHLANDER 2020", + [ + ToyotaCarDocs("Toyota Highlander 2020-23"), + ToyotaCarDocs("Toyota Highlander Hybrid 2020-23"), + ], + HIGHLANDER.specs, + ) + PRIUS = PlatformConfig( + "TOYOTA PRIUS 2017", + [ + ToyotaCarDocs("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), + ToyotaCarDocs("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), + ToyotaCarDocs("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), + ], + CarSpecs(mass=3045. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.74, tireStiffnessFactor=0.6371), + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + ) + PRIUS_V = PlatformConfig( + "TOYOTA PRIUS v 2017", + [ToyotaCarDocs("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED)], + CarSpecs(mass=3340. * CV.LB_TO_KG, wheelbase=2.78, steerRatio=17.4, tireStiffnessFactor=0.5533), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU, + ) + PRIUS_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA PRIUS TSS2 2021", + [ + ToyotaCarDocs("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), + ToyotaCarDocs("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), + ], + CarSpecs(mass=3115. * CV.LB_TO_KG, wheelbase=2.70002, steerRatio=13.4, tireStiffnessFactor=0.6371), + ) + RAV4 = PlatformConfig( + "TOYOTA RAV4 2017", + [ + ToyotaCarDocs("Toyota RAV4 2016", "Toyota Safety Sense P"), + ToyotaCarDocs("Toyota RAV4 2017-18") + ], + CarSpecs(mass=3650. * CV.LB_TO_KG, wheelbase=2.65, steerRatio=16.88, tireStiffnessFactor=0.5533), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + RAV4H = PlatformConfig( + "TOYOTA RAV4 HYBRID 2017", + [ + ToyotaCarDocs("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26") + ], + RAV4.specs, + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER, + ) + RAV4_TSS2 = ToyotaTSS2PlatformConfig( + "TOYOTA RAV4 2019", + [ + ToyotaCarDocs("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2019-21"), + ], + CarSpecs(mass=3585. * CV.LB_TO_KG, wheelbase=2.68986, steerRatio=14.3, tireStiffnessFactor=0.7933), + ) + RAV4_TSS2_2022 = ToyotaTSS2PlatformConfig( + "TOYOTA RAV4 2022", + [ + ToyotaCarDocs("Toyota RAV4 2022"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), + ], + RAV4_TSS2.specs, + flags=ToyotaFlags.RADAR_ACC, + ) + RAV4_TSS2_2023 = ToyotaTSS2PlatformConfig( + "TOYOTA RAV4 2023", + [ + ToyotaCarDocs("Toyota RAV4 2023-24"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2023-24"), + ], + RAV4_TSS2.specs, + flags=ToyotaFlags.RADAR_ACC | ToyotaFlags.ANGLE_CONTROL, + ) + MIRAI = ToyotaTSS2PlatformConfig( + "TOYOTA MIRAI 2021", # TSS 2.5 + [ToyotaCarDocs("Toyota Mirai 2021")], + CarSpecs(mass=4300. * CV.LB_TO_KG, wheelbase=2.91, steerRatio=14.8, tireStiffnessFactor=0.8), + ) + SIENNA = PlatformConfig( + "TOYOTA SIENNA 2018", + [ToyotaCarDocs("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED)], + CarSpecs(mass=4590. * CV.LB_TO_KG, wheelbase=3.03, steerRatio=15.5, tireStiffnessFactor=0.444), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER, + ) # Lexus - CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"), - CAR.LEXUS_ES: [ - ToyotaCarInfo("Lexus ES 2017-18"), - ToyotaCarInfo("Lexus ES Hybrid 2017-18"), - ], - CAR.LEXUS_ES_TSS2: [ - ToyotaCarInfo("Lexus ES 2019-24"), - ToyotaCarInfo("Lexus ES Hybrid 2019-24", video_link="https://youtu.be/BZ29osRVJeg?t=12"), - ], - CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), - CAR.LEXUS_IS_TSS2: ToyotaCarInfo("Lexus IS 2022-23"), - CAR.LEXUS_GS_F: ToyotaCarInfo("Lexus GS F 2016"), - CAR.LEXUS_NX: [ - ToyotaCarInfo("Lexus NX 2018-19"), - ToyotaCarInfo("Lexus NX Hybrid 2018-19"), - ], - CAR.LEXUS_NX_TSS2: [ - ToyotaCarInfo("Lexus NX 2020-21"), - ToyotaCarInfo("Lexus NX Hybrid 2020-21"), - ], - CAR.LEXUS_LC_TSS2: ToyotaCarInfo("Lexus LC 2024"), - CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"), - CAR.LEXUS_RX: [ - ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"), - ToyotaCarInfo("Lexus RX 2017-19"), - # Hybrid platforms - ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+"), - ToyotaCarInfo("Lexus RX Hybrid 2017-19"), - ], - CAR.LEXUS_RX_TSS2: [ - ToyotaCarInfo("Lexus RX 2020-22"), - ToyotaCarInfo("Lexus RX Hybrid 2020-22"), - ], -} + LEXUS_CTH = PlatformConfig( + "LEXUS CT HYBRID 2018", + [ToyotaCarDocs("Lexus CT Hybrid 2017-18", "Lexus Safety System+")], + CarSpecs(mass=3108. * CV.LB_TO_KG, wheelbase=2.6, steerRatio=18.6, tireStiffnessFactor=0.517), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + LEXUS_ES = PlatformConfig( + "LEXUS ES 2018", + [ + ToyotaCarDocs("Lexus ES 2017-18"), + ToyotaCarDocs("Lexus ES Hybrid 2017-18"), + ], + CarSpecs(mass=3677. * CV.LB_TO_KG, wheelbase=2.8702, steerRatio=16.0, tireStiffnessFactor=0.444), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + LEXUS_ES_TSS2 = ToyotaTSS2PlatformConfig( + "LEXUS ES 2019", + [ + ToyotaCarDocs("Lexus ES 2019-24"), + ToyotaCarDocs("Lexus ES Hybrid 2019-24", video_link="https://youtu.be/BZ29osRVJeg?t=12"), + ], + LEXUS_ES.specs, + ) + LEXUS_IS = PlatformConfig( + "LEXUS IS 2018", + [ToyotaCarDocs("Lexus IS 2017-19")], + CarSpecs(mass=3736.8 * CV.LB_TO_KG, wheelbase=2.79908, steerRatio=13.3, tireStiffnessFactor=0.444), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.UNSUPPORTED_DSU, + ) + LEXUS_IS_TSS2 = ToyotaTSS2PlatformConfig( + "LEXUS IS 2023", + [ToyotaCarDocs("Lexus IS 2022-23")], + LEXUS_IS.specs, + ) + LEXUS_NX = PlatformConfig( + "LEXUS NX 2018", + [ + ToyotaCarDocs("Lexus NX 2018-19"), + ToyotaCarDocs("Lexus NX Hybrid 2018-19"), + ], + CarSpecs(mass=4070. * CV.LB_TO_KG, wheelbase=2.66, steerRatio=14.7, tireStiffnessFactor=0.444), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + ) + LEXUS_NX_TSS2 = ToyotaTSS2PlatformConfig( + "LEXUS NX 2020", + [ + ToyotaCarDocs("Lexus NX 2020-21"), + ToyotaCarDocs("Lexus NX Hybrid 2020-21"), + ], + LEXUS_NX.specs, + ) + LEXUS_LC_TSS2 = ToyotaTSS2PlatformConfig( + "LEXUS LC 2024", + [ToyotaCarDocs("Lexus LC 2024")], + CarSpecs(mass=4500. * CV.LB_TO_KG, wheelbase=2.87, steerRatio=13.0, tireStiffnessFactor=0.444), + ) + LEXUS_RC = PlatformConfig( + "LEXUS RC 2020", + [ToyotaCarDocs("Lexus RC 2018-20")], + LEXUS_IS.specs, + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.UNSUPPORTED_DSU, + ) + LEXUS_RX = PlatformConfig( + "LEXUS RX 2016", + [ + ToyotaCarDocs("Lexus RX 2016", "Lexus Safety System+"), + ToyotaCarDocs("Lexus RX 2017-19"), + # Hybrid platforms + ToyotaCarDocs("Lexus RX Hybrid 2016", "Lexus Safety System+"), + ToyotaCarDocs("Lexus RX Hybrid 2017-19"), + ], + CarSpecs(mass=4481. * CV.LB_TO_KG, wheelbase=2.79, steerRatio=16., tireStiffnessFactor=0.5533), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + ) + LEXUS_RX_TSS2 = ToyotaTSS2PlatformConfig( + "LEXUS RX 2020", + [ + ToyotaCarDocs("Lexus RX 2020-22"), + ToyotaCarDocs("Lexus RX Hybrid 2020-22"), + ], + LEXUS_RX.specs, + ) + LEXUS_GS_F = PlatformConfig( + "LEXUS GS F 2016", + [ToyotaCarDocs("Lexus GS F 2016")], + CarSpecs(mass=4034. * CV.LB_TO_KG, wheelbase=2.84988, steerRatio=13.3, tireStiffnessFactor=0.444), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.UNSUPPORTED_DSU, + ) + # (addr, cars, bus, 1/freq*100, vl) STATIC_DSU_MSGS = [ @@ -255,7 +405,7 @@ STATIC_DSU_MSGS = [ ] -def get_platform_codes(fw_versions: List[bytes]) -> Dict[bytes, Set[bytes]]: +def get_platform_codes(fw_versions: list[bytes]) -> dict[bytes, set[bytes]]: # Returns sub versions in a dict so comparisons can be made within part-platform-major_version combos codes = defaultdict(set) # Optional[part]-platform-major_version: set of sub_version for fw in fw_versions: @@ -299,7 +449,7 @@ def get_platform_codes(fw_versions: List[bytes]) -> Dict[bytes, Set[bytes]]: return dict(codes) -def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> Set[str]: +def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> set[str]: candidates = set() for candidate, fws in offline_fw_versions.items(): @@ -402,7 +552,7 @@ FW_QUERY_CONFIG = FwQueryConfig( Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS, CAR.ALPHARD_TSS2], # On some models, the engine can show on two different addresses Ecu.engine: [CAR.HIGHLANDER, CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, - CAR.LEXUS_RC, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2], + CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2], }, extra_ecus=[ # All known ECUs on a late-model Toyota vehicle not queried here: @@ -442,66 +592,26 @@ FW_QUERY_CONFIG = FwQueryConfig( STEER_THRESHOLD = 100 -DBC = { - CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.LEXUS_LC_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_ES: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_IS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_GS_F: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), -} - # These cars have non-standard EPS torque scale factors. All others are 73 EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 -TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.LEXUS_ES_TSS2, - CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.LEXUS_IS_TSS2, - CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_LC_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, - CAR.CHR_TSS2} +TSS2_CAR = CAR.with_flags(ToyotaFlags.TSS2) -NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY} +NO_DSU_CAR = CAR.with_flags(ToyotaFlags.NO_DSU) # the DSU uses the AEB message for longitudinal on these cars -UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC, CAR.LEXUS_GS_F} +UNSUPPORTED_DSU_CAR = CAR.with_flags(ToyotaFlags.UNSUPPORTED_DSU) # these cars have a radar which sends ACC messages instead of the camera -RADAR_ACC_CAR = {CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2} +RADAR_ACC_CAR = CAR.with_flags(ToyotaFlags.RADAR_ACC) -# these cars use the Lane Tracing Assist (LTA) message for lateral control -ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023} +ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL) # no resume button press required -NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDER, CAR.SIENNA} +NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) -# stop and go STOP_AND_GO_CAR = TSS2_CAR | {CAR.PRIUS, CAR.PRIUS_V, CAR.RAV4H, CAR.LEXUS_RX, CAR.CHR, CAR.CAMRY, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX, CAR.MIRAI, CAR.AVALON_2019} + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/values.py b/selfdrive/car/values.py new file mode 100644 index 0000000..0c82498 --- /dev/null +++ b/selfdrive/car/values.py @@ -0,0 +1,25 @@ +from typing import Any, Callable, cast +from openpilot.selfdrive.car.body.values import CAR as BODY +from openpilot.selfdrive.car.chrysler.values import CAR as CHRYSLER +from openpilot.selfdrive.car.ford.values import CAR as FORD +from openpilot.selfdrive.car.gm.values import CAR as GM +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI +from openpilot.selfdrive.car.mazda.values import CAR as MAZDA +from openpilot.selfdrive.car.mock.values import CAR as MOCK +from openpilot.selfdrive.car.nissan.values import CAR as NISSAN +from openpilot.selfdrive.car.subaru.values import CAR as SUBARU +from openpilot.selfdrive.car.tesla.values import CAR as TESLA +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN + +Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN +BRANDS = [BODY, CHRYSLER, FORD, GM, HONDA, HYUNDAI, MAZDA, MOCK, NISSAN, SUBARU, TESLA, TOYOTA, VOLKSWAGEN] + +PLATFORMS: dict[str, Platform] = {str(platform): platform for brand in BRANDS for platform in cast(list[Platform], brand)} + +MapFunc = Callable[[Platform], Any] + + +def create_platform_map(func: MapFunc): + return {str(platform): func(platform) for platform in PLATFORMS.values() if func(platform) is not None} diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index d3d6f70..ba1ee43 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -4,18 +4,19 @@ from openpilot.common.numpy_fast import clip from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan -from openpilot.selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams, VolkswagenFlags +from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.CCP = CarControllerParams(CP) - self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan + self.CCS = pqcan if CP.flags & VolkswagenFlags.PQ else mqbcan self.packer_pt = CANPacker(dbc_name) self.apply_steer_last = 0 @@ -104,7 +105,7 @@ class CarController: # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? set_speed = hud_control.setSpeed * CV.MS_TO_KPH can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - lead_distance, CS.personality_profile)) + lead_distance, hud_control.leadDistanceBars)) # **** Stock ACC Button Controls **************************************** # diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index c130cd2..516bd62 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -3,13 +3,15 @@ from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \ +from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ CarControllerParams, VolkswagenFlags class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) + self.frame = 0 + self.eps_init_complete = False self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False @@ -31,7 +33,7 @@ class CarState(CarStateBase): return button_events def update(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): - if self.CP.carFingerprint in PQ_CARS: + if self.CP.flags & VolkswagenFlags.PQ: return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables) ret = car.CarState.new_message() @@ -47,18 +49,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 - # Update steering angle, rate, yaw rate, and driver input torque. VW send - # the sign/direction in a separate signal so they must be recombined. + # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["ESP_02"]["ESP_Gierrate"] * (1, -1)[int(pt_cp.vl["ESP_02"]["ESP_VZ_Gierrate"])] * CV.DEG_TO_RAD - - # Verify EPS readiness to accept steering commands hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) - ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") - ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) # VW Emergency Assist status tracking and mitigation self.eps_stock_values = pt_cp.vl["LH_EPS_03"] @@ -151,25 +149,7 @@ class CarState(CarStateBase): # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) - # Driving personalities function - if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: - # Sync with the onroad UI button - if self.fpf.personality_changed_via_ui: - self.personality_profile = self.fpf.current_personality - self.previous_personality_profile = self.personality_profile - self.fpf.reset_personality_changed_param() - - # Change personality upon steering wheel button press - distance_button = pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"] - - if distance_button and not self.distance_previously_pressed: - self.personality_profile = (self.previous_personality_profile + 2) % 3 - self.distance_previously_pressed = distance_button - - if self.personality_profile != self.previous_personality_profile: - self.fpf.distance_button_function(self.personality_profile) - self.previous_personality_profile = self.personality_profile - + self.frame += 1 return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): @@ -187,18 +167,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 - # Update steering angle, rate, yaw rate, and driver input torque. VW send - # the sign/direction in a separate signal so they must be recombined. + # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. ret.steeringAngleDeg = pt_cp.vl["Lenkhilfe_3"]["LH3_BLW"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"])] ret.steeringRateDeg = pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit_S"])] ret.steeringTorque = pt_cp.vl["Lenkhilfe_3"]["LH3_LM"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_LMSign"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD - - # Verify EPS readiness to accept steering commands hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"]) - ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") - ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) # Update gas, brakes, and gearshift. ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 @@ -272,11 +248,20 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) + self.frame += 1 return ret + def update_hca_state(self, hca_status): + # Treat INITIALIZING and FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist + # DISABLED means the EPS hasn't been configured to support Lane Assist + self.eps_init_complete = self.eps_init_complete or (hca_status in ("DISABLED", "READY", "ACTIVE") or self.frame > 600) + perm_fault = hca_status == "DISABLED" or (self.eps_init_complete and hca_status in ("INITIALIZING", "FAULT")) + temp_fault = hca_status == "REJECTED" or not self.eps_init_complete + return temp_fault, perm_fault + @staticmethod def get_can_parser(CP): - if CP.carFingerprint in PQ_CARS: + if CP.flags & VolkswagenFlags.PQ: return CarState.get_can_parser_pq(CP) messages = [ @@ -313,7 +298,7 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): - if CP.carFingerprint in PQ_CARS: + if CP.flags & VolkswagenFlags.PQ: return CarState.get_cam_can_parser_pq(CP) messages = [] diff --git a/selfdrive/car/volkswagen/fingerprints.py b/selfdrive/car/volkswagen/fingerprints.py index eab2bc0..e39e65b 100644 --- a/selfdrive/car/volkswagen/fingerprints.py +++ b/selfdrive/car/volkswagen/fingerprints.py @@ -80,6 +80,7 @@ FW_VERSIONS = { b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\x0e2214152212001105141122052900', b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\x0e1114151112001105111122052900', b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\x0e2214152212001105141122052900', + b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105111122052J00', b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00', b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1115151112001105171122052J00', ], @@ -87,6 +88,7 @@ FW_VERSIONS = { b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B60924A1', b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1', b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6M921A1', + b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6N920A1', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6080105', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', ], @@ -99,6 +101,17 @@ FW_VERSIONS = { b'\xf1\x875Q0907572P \xf1\x890682', ], }, + CAR.CADDY_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027T \xf1\x892363', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872K5959655E \xf1\x890018\xf1\x82\x05000P037605', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0155', + ], + }, CAR.CRAFTER_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906056BP\xf1\x894729', @@ -164,6 +177,7 @@ FW_VERSIONS = { b'\xf1\x875G0906259T \xf1\x890003', b'\xf1\x878V0906259H \xf1\x890002', b'\xf1\x878V0906259J \xf1\x890003', + b'\xf1\x878V0906259J \xf1\x890103', b'\xf1\x878V0906259K \xf1\x890001', b'\xf1\x878V0906259K \xf1\x890003', b'\xf1\x878V0906259P \xf1\x890001', @@ -209,6 +223,7 @@ FW_VERSIONS = { b'\xf1\x870DD300046F \xf1\x891601', b'\xf1\x870GC300012A \xf1\x891401', b'\xf1\x870GC300012A \xf1\x891403', + b'\xf1\x870GC300012M \xf1\x892301', b'\xf1\x870GC300014B \xf1\x892401', b'\xf1\x870GC300014B \xf1\x892403', b'\xf1\x870GC300014B \xf1\x892405', @@ -227,6 +242,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\x13141500111233003142114A2131219333313100', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333423100', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100', + b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x13141600111233003142115A2232229333463100', b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2252229333463100', @@ -317,6 +333,7 @@ FW_VERSIONS = { b'\xf1\x8704E906024BC\xf1\x899971', b'\xf1\x8704E906024BG\xf1\x891057', b'\xf1\x8704E906024C \xf1\x899970', + b'\xf1\x8704E906024C \xf1\x899971', b'\xf1\x8704E906024L \xf1\x895595', b'\xf1\x8704E906024L \xf1\x899970', b'\xf1\x8704E906027MS\xf1\x896223', @@ -370,6 +387,7 @@ FW_VERSIONS = { b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x8705L906022A \xf1\x890827', b'\xf1\x873G0906259 \xf1\x890004', b'\xf1\x873G0906259B \xf1\x890002', b'\xf1\x873G0906264 \xf1\x890004', @@ -389,6 +407,7 @@ FW_VERSIONS = { b'\xf1\x870DL300011H \xf1\x895201', b'\xf1\x870GC300042H \xf1\x891404', b'\xf1\x870GC300043 \xf1\x892301', + b'\xf1\x870GC300046P \xf1\x892805', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', @@ -404,6 +423,7 @@ FW_VERSIONS = { b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e5915005914001354701311542900', b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e5915005914001305701311052900', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011111200631145171716121691132111', + b'\xf1\x875QF959655S \xf1\x890639\xf1\x82\x13131100131300111111000120----2211114A48', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00611A1', @@ -564,6 +584,7 @@ FW_VERSIONS = { b'\xf1\x8709G927158GM\xf1\x893936', b'\xf1\x8709G927158GN\xf1\x893938', b'\xf1\x8709G927158HB\xf1\x894069', + b'\xf1\x8709G927158HC\xf1\x894070', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DD300046K \xf1\x892302', b'\xf1\x870DL300011N \xf1\x892001', @@ -622,24 +643,32 @@ FW_VERSIONS = { }, CAR.TOURAN_MK2: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906025BE\xf1\x890720', + b'\xf1\x8704E906027HQ\xf1\x893746', b'\xf1\x8704L906026HM\xf1\x893017', b'\xf1\x8705E906018CQ\xf1\x890808', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300020A \xf1\x891936', b'\xf1\x870CW300041E \xf1\x891005', + b'\xf1\x870CW300041Q \xf1\x891606', b'\xf1\x870CW300051M \xf1\x891926', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\x1336350021353335314132014730479333313100', b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\x13363500213533353141324C4732479333313100', b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1336350021353336314740025250529333613100', + b'\xf1\x875QD959655AJ\xf1\x890421\xf1\x82\x1336350021313300314240023330339333663100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A8090400', + b'\xf1\x875QD909144F \xf1\x891082\xf1\x82\x0521A00642A1', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x875Q0907572R \xf1\x890771', ], }, CAR.TRANSPORTER_T61: { @@ -938,6 +967,7 @@ FW_VERSIONS = { }, CAR.SKODA_KAROQ_MK1: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705E906013CL\xf1\x892541', b'\xf1\x8705E906013H \xf1\x892407', b'\xf1\x8705E906018P \xf1\x895472', b'\xf1\x8705E906018P \xf1\x896020', @@ -957,6 +987,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563T6090500', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100500', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100600', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100700', ], (Ecu.fwdRadar, 0x757, None): [ @@ -977,7 +1008,9 @@ FW_VERSIONS = { b'\xf1\x8704L906026HT\xf1\x893617', b'\xf1\x8705E906018DJ\xf1\x890915', b'\xf1\x8705E906018DJ\xf1\x891903', + b'\xf1\x8705L906022GM\xf1\x893411', b'\xf1\x875NA906259E \xf1\x890003', + b'\xf1\x875NA907115D \xf1\x890003', b'\xf1\x875NA907115E \xf1\x890003', b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115E \xf1\x890001', @@ -986,14 +1019,17 @@ FW_VERSIONS = { b'\xf1\x870D9300014S \xf1\x895201', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892014', + b'\xf1\x870DL300012G \xf1\x892006', b'\xf1\x870DL300012M \xf1\x892107', b'\xf1\x870DL300012N \xf1\x892110', b'\xf1\x870DL300013G \xf1\x892119', b'\xf1\x870GC300014N \xf1\x892801', + b'\xf1\x870GC300018S \xf1\x892803', b'\xf1\x870GC300019H \xf1\x892806', b'\xf1\x870GC300046Q \xf1\x892802', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r11110011110011031111310311', b'\xf1\x873Q0959655AP\xf1\x890306\xf1\x82\r11110011110011421111314211', b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1213001211001205212111052100', b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\x0e1213001211001205212111052100', @@ -1002,6 +1038,7 @@ FW_VERSIONS = { b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111', b'\xf1\x873Q0959655DJ\xf1\x890731\xf1\x82\x0e1513001511001205232113052J00', b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1311110011110011111100010200--1121240749', + b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1311110011110011111100010200--1121246149', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405', @@ -1009,6 +1046,7 @@ FW_VERSIONS = { b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6070405', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G500', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600', + b'\xf1\x875TA907145F \xf1\x891063\xf1\x82\x0025T6BA25OM', b'\xf1\x875TA907145F \xf1\x891063\xf1\x82\x002LT61A2LOM', ], (Ecu.fwdRadar, 0x757, None): [ @@ -1027,6 +1065,7 @@ FW_VERSIONS = { b'\xf1\x8704E906027HD\xf1\x893742', b'\xf1\x8704E906027MH\xf1\x894786', b'\xf1\x8704L906021DT\xf1\x898127', + b'\xf1\x8704L906021ER\xf1\x898361', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906026BS\xf1\x891541', b'\xf1\x875G0906259C \xf1\x890002', @@ -1036,6 +1075,7 @@ FW_VERSIONS = { b'\xf1\x870CW300041N \xf1\x891605', b'\xf1\x870CW300043B \xf1\x891601', b'\xf1\x870CW300043P \xf1\x891605', + b'\xf1\x870D9300012H \xf1\x894518', b'\xf1\x870D9300041C \xf1\x894936', b'\xf1\x870D9300041H \xf1\x895220', b'\xf1\x870D9300041J \xf1\x894902', @@ -1061,6 +1101,7 @@ FW_VERSIONS = { b'\xf1\x875QD909144E \xf1\x891081\xf1\x82\x0521T00503A1', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907567P \xf1\x890100\xf1\x82\x0101', b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101', b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101', b'\xf1\x875Q0907572H \xf1\x890620', diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 30df8d0..9b9f4e0 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,9 +1,8 @@ from cereal import car from panda import Panda -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter, VolkswagenFlags +from openpilot.selfdrive.car.volkswagen.values import CAR, CANBUS, NetworkLocation, TransmissionType, GearShifter, VolkswagenFlags ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -23,11 +22,11 @@ class CarInterface(CarInterfaceBase): self.eps_timer_soft_disable_alert = False @staticmethod - def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "volkswagen" ret.radarUnavailable = True - if candidate in PQ_CARS: + if ret.flags & VolkswagenFlags.PQ: # Set global PQ35/PQ46/NMS parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)] ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1 @@ -72,21 +71,24 @@ class CarInterface(CarInterfaceBase): # Global lateral tuning defaults, can be overridden per-vehicle - ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 - ret.steerRatio = 15.6 # Let the params learner figure this out - ret.lateralTuning.pid.kpBP = [0.] - ret.lateralTuning.pid.kiBP = [0.] - ret.lateralTuning.pid.kf = 0.00006 - ret.lateralTuning.pid.kpV = [0.6] - ret.lateralTuning.pid.kiV = [0.2] + if ret.flags & VolkswagenFlags.PQ: + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + else: + ret.steerActuatorDelay = 0.1 + ret.lateralTuning.pid.kpBP = [0.] + ret.lateralTuning.pid.kiBP = [0.] + ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kpV = [0.6] + ret.lateralTuning.pid.kiV = [0.2] # Global longitudinal tuning defaults, can be overridden per-vehicle ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or docs if experimental_long: # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required. - ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal") + ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL if ret.transmissionType == TransmissionType.manual: ret.minEnableSpeed = 4.5 @@ -98,137 +100,15 @@ class CarInterface(CarInterfaceBase): ret.vEgoStopping = 0.5 ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] - - # Per-chassis tuning values, override tuning defaults here if desired - - if candidate == CAR.ARTEON_MK1: - ret.mass = 1733 - ret.wheelbase = 2.84 - - elif candidate == CAR.ATLAS_MK1: - ret.mass = 2011 - ret.wheelbase = 2.98 - - elif candidate == CAR.CRAFTER_MK2: - ret.mass = 2100 - ret.wheelbase = 3.64 # SWB, LWB is 4.49, TBD how to detect difference - ret.minSteerSpeed = 50 * CV.KPH_TO_MS - - elif candidate == CAR.GOLF_MK7: - ret.mass = 1397 - ret.wheelbase = 2.62 - - elif candidate == CAR.JETTA_MK7: - ret.mass = 1328 - ret.wheelbase = 2.71 - - elif candidate == CAR.PASSAT_MK8: - ret.mass = 1551 - ret.wheelbase = 2.79 - - elif candidate == CAR.PASSAT_NMS: - ret.mass = 1503 - ret.wheelbase = 2.80 - ret.minEnableSpeed = 20 * CV.KPH_TO_MS # ACC "basic", no FtS - ret.minSteerSpeed = 50 * CV.KPH_TO_MS - ret.steerActuatorDelay = 0.2 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - - elif candidate == CAR.POLO_MK6: - ret.mass = 1230 - ret.wheelbase = 2.55 - - elif candidate == CAR.SHARAN_MK2: - ret.mass = 1639 - ret.wheelbase = 2.92 - ret.minSteerSpeed = 50 * CV.KPH_TO_MS - ret.steerActuatorDelay = 0.2 - - elif candidate == CAR.TAOS_MK1: - ret.mass = 1498 - ret.wheelbase = 2.69 - - elif candidate == CAR.TCROSS_MK1: - ret.mass = 1150 - ret.wheelbase = 2.60 - - elif candidate == CAR.TIGUAN_MK2: - ret.mass = 1715 - ret.wheelbase = 2.74 - - elif candidate == CAR.TOURAN_MK2: - ret.mass = 1516 - ret.wheelbase = 2.79 - - elif candidate == CAR.TRANSPORTER_T61: - ret.mass = 1926 - ret.wheelbase = 3.00 # SWB, LWB is 3.40, TBD how to detect difference - ret.minSteerSpeed = 14.0 - - elif candidate == CAR.TROC_MK1: - ret.mass = 1413 - ret.wheelbase = 2.63 - - elif candidate == CAR.AUDI_A3_MK3: - ret.mass = 1335 - ret.wheelbase = 2.61 - - elif candidate == CAR.AUDI_Q2_MK1: - ret.mass = 1205 - ret.wheelbase = 2.61 - - elif candidate == CAR.AUDI_Q3_MK2: - ret.mass = 1623 - ret.wheelbase = 2.68 - - elif candidate == CAR.SEAT_ATECA_MK1: - ret.mass = 1900 - ret.wheelbase = 2.64 - - elif candidate == CAR.SEAT_LEON_MK3: - ret.mass = 1227 - ret.wheelbase = 2.64 - - elif candidate == CAR.SKODA_FABIA_MK4: - ret.mass = 1266 - ret.wheelbase = 2.56 - - elif candidate == CAR.SKODA_KAMIQ_MK1: - ret.mass = 1265 - ret.wheelbase = 2.66 - - elif candidate == CAR.SKODA_KAROQ_MK1: - ret.mass = 1278 - ret.wheelbase = 2.66 - - elif candidate == CAR.SKODA_KODIAQ_MK1: - ret.mass = 1569 - ret.wheelbase = 2.79 - - elif candidate == CAR.SKODA_OCTAVIA_MK3: - ret.mass = 1388 - ret.wheelbase = 2.68 - - elif candidate == CAR.SKODA_SCALA_MK1: - ret.mass = 1192 - ret.wheelbase = 2.65 - - elif candidate == CAR.SKODA_SUPERB_MK3: - ret.mass = 1505 - ret.wheelbase = 2.84 - - else: - raise ValueError(f"unsupported car {candidate}") - ret.autoResumeSng = ret.minEnableSpeed == -1 - ret.centerToFront = ret.wheelbase * 0.45 + return ret # returns a car.CarState def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType, frogpilot_variables) - events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], + events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=not self.CS.CP.openpilotLongitudinalControl, enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index ae2d195..a0aa144 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -125,11 +125,11 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, personality_profile): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, - "ACC_Gesetzte_Zeitluecke": 1 if personality_profile == 0 else 3 if personality_profile == 1 else 5, + "ACC_Gesetzte_Zeitluecke": distance + 2, "ACC_Display_Prio": 3, "ACC_Abstandsindex": lead_distance, } diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 6a43932..8b97ba4 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -91,10 +91,10 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, personality_profile): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): values = { "ACA_StaACC": acc_hud_status, - "ACA_Zeitluecke": 2, + "ACA_Zeitluecke": distance + 2, "ACA_V_Wunsch": set_speed, "ACA_gemZeitl": lead_distance, "ACA_PrioDisp": 3, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index e44abbf..57c7c0a 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,13 +1,13 @@ -from collections import defaultdict, namedtuple +from collections import namedtuple from dataclasses import dataclass, field -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Union +from enum import Enum, IntFlag from cereal import car from panda.python import uds from opendbc.can.can_define import CANDefine -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \ +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import dbc_dict, CarSpecs, DbcDict, PlatformConfig, Platforms +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \ Device from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 @@ -41,7 +41,7 @@ class CarControllerParams: def __init__(self, CP): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - if CP.carFingerprint in PQ_CARS: + if CP.flags & VolkswagenFlags.PQ: self.LDW_STEP = 5 # LDW_1 message frequency 20Hz self.ACC_HUD_STEP = 4 # ACC_GRA_Anzeige frequency 25Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm @@ -111,50 +111,30 @@ class CANBUS: class VolkswagenFlags(IntFlag): + # Detected flags STOCK_HCA_PRESENT = 1 - -# Check the 7th and 8th characters of the VIN before adding a new CAR. If the -# chassis code is already listed below, don't add a new CAR, just add to the -# FW_VERSIONS for that existing CAR. -# Exception: SEAT Leon and SEAT Ateca share a chassis code - -class CAR(StrEnum): - ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants - ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport - CRAFTER_MK2 = "VOLKSWAGEN CRAFTER 2ND GEN" # Chassis SY/SZ, Mk2 VW Crafter, VW Grand California, MAN TGE - GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants - JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta - PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants - PASSAT_NMS = "VOLKSWAGEN PASSAT NMS" # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift - POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo - SHARAN_MK2 = "VOLKSWAGEN SHARAN 2ND GEN" # Chassis 7N, Mk2 Volkswagen Sharan and SEAT Alhambra - TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu - TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants - TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants - TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants - TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California - TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW T-Roc and variants - AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants - AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only) - AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants - SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca - SEAT_LEON_MK3 = "SEAT LEON 3RD GEN" # Chassis 5F, Mk3 SEAT Leon and variants - SKODA_FABIA_MK4 = "SKODA FABIA 4TH GEN" # Chassis PJ, Mk4 Skoda Fabia - SKODA_KAMIQ_MK1 = "SKODA KAMIQ 1ST GEN" # Chassis NW, Mk1 Skoda Kamiq - SKODA_KAROQ_MK1 = "SKODA KAROQ 1ST GEN" # Chassis NU, Mk1 Skoda Karoq - SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN" # Chassis NS, Mk1 Skoda Kodiaq - SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN" # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq - SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants - SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants + # Static flags + PQ = 2 -PQ_CARS = {CAR.PASSAT_NMS, CAR.SHARAN_MK2} +@dataclass +class VolkswagenMQBPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('vw_mqb_2010', None)) -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None)) -for car_type in PQ_CARS: - DBC[car_type] = dbc_dict("vw_golf_mk4", None) +@dataclass +class VolkswagenPQPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('vw_golf_mk4', None)) + + def init(self): + self.flags |= VolkswagenFlags.PQ + + +@dataclass(frozen=True, kw_only=True) +class VolkswagenCarSpecs(CarSpecs): + centerToFrontRatio: float = 0.45 + steerRatio: float = 15.6 class Footnote(Enum): @@ -179,7 +159,7 @@ class Footnote(Enum): @dataclass -class VWCarInfo(CarInfo): +class VWCarDocs(CarDocs): package: str = "Adaptive Cruise Control (ACC) & Lane Assist" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.j533])) @@ -192,88 +172,208 @@ class VWCarInfo(CarInfo): self.car_parts = CarParts([Device.threex_angled_mount, CarHarness.j533]) -CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { - CAR.ARTEON_MK1: [ - VWCarInfo("Volkswagen Arteon 2018-23", video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen Arteon R 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen Arteon eHybrid 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"), - ], - CAR.ATLAS_MK1: [ - VWCarInfo("Volkswagen Atlas 2018-23"), - VWCarInfo("Volkswagen Atlas Cross Sport 2020-22"), - VWCarInfo("Volkswagen Teramont 2018-22"), - VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"), - VWCarInfo("Volkswagen Teramont X 2021-22"), - ], - CAR.CRAFTER_MK2: [ - VWCarInfo("Volkswagen Crafter 2017-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("Volkswagen e-Crafter 2018-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("Volkswagen Grand California 2019-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("MAN TGE 2017-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("MAN eTGE 2020-23", video_link="https://youtu.be/4100gLeabmo"), - ], - CAR.GOLF_MK7: [ - VWCarInfo("Volkswagen e-Golf 2014-20"), - VWCarInfo("Volkswagen Golf 2015-20", auto_resume=False), - VWCarInfo("Volkswagen Golf Alltrack 2015-19", auto_resume=False), - VWCarInfo("Volkswagen Golf GTD 2015-20"), - VWCarInfo("Volkswagen Golf GTE 2015-20"), - VWCarInfo("Volkswagen Golf GTI 2015-21", auto_resume=False), - VWCarInfo("Volkswagen Golf R 2015-19"), - VWCarInfo("Volkswagen Golf SportsVan 2015-20"), - ], - CAR.JETTA_MK7: [ - VWCarInfo("Volkswagen Jetta 2018-24"), - VWCarInfo("Volkswagen Jetta GLI 2021-24"), - ], - CAR.PASSAT_MK8: [ - VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]), - VWCarInfo("Volkswagen Passat Alltrack 2015-22"), - VWCarInfo("Volkswagen Passat GTE 2015-22"), - ], - CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22"), - CAR.POLO_MK6: [ - VWCarInfo("Volkswagen Polo 2018-23", footnotes=[Footnote.VW_MQB_A0]), - VWCarInfo("Volkswagen Polo GTI 2018-23", footnotes=[Footnote.VW_MQB_A0]), - ], - CAR.SHARAN_MK2: [ - VWCarInfo("Volkswagen Sharan 2018-22"), - VWCarInfo("SEAT Alhambra 2018-20"), - ], - CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022-23"), - CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]), - CAR.TIGUAN_MK2: [ - VWCarInfo("Volkswagen Tiguan 2018-24"), - VWCarInfo("Volkswagen Tiguan eHybrid 2021-23"), - ], - CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2016-23"), - CAR.TRANSPORTER_T61: [ - VWCarInfo("Volkswagen Caravelle 2020"), - VWCarInfo("Volkswagen California 2021-23"), - ], - CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2018-22", footnotes=[Footnote.VW_MQB_A0]), - CAR.AUDI_A3_MK3: [ - VWCarInfo("Audi A3 2014-19"), - VWCarInfo("Audi A3 Sportback e-tron 2017-18"), - VWCarInfo("Audi RS3 2018"), - VWCarInfo("Audi S3 2015-17"), - ], - CAR.AUDI_Q2_MK1: VWCarInfo("Audi Q2 2018"), - CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2019-23"), - CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"), - CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"), - CAR.SKODA_FABIA_MK4: VWCarInfo("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0]), - CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021-23", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]), - CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-23"), - CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2017-23"), - CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0]), - CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"), - CAR.SKODA_OCTAVIA_MK3: [ - VWCarInfo("Škoda Octavia 2015-19"), - VWCarInfo("Škoda Octavia RS 2016"), - ], -} +# Check the 7th and 8th characters of the VIN before adding a new CAR. If the +# chassis code is already listed below, don't add a new CAR, just add to the +# FW_VERSIONS for that existing CAR. +# Exception: SEAT Leon and SEAT Ateca share a chassis code + +class CAR(Platforms): + ARTEON_MK1 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN ARTEON 1ST GEN", # Chassis AN + [ + VWCarDocs("Volkswagen Arteon 2018-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen Arteon R 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen Arteon eHybrid 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"), + ], + VolkswagenCarSpecs(mass=1733, wheelbase=2.84), + ) + ATLAS_MK1 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN ATLAS 1ST GEN", # Chassis CA + [ + VWCarDocs("Volkswagen Atlas 2018-23"), + VWCarDocs("Volkswagen Atlas Cross Sport 2020-22"), + VWCarDocs("Volkswagen Teramont 2018-22"), + VWCarDocs("Volkswagen Teramont Cross Sport 2021-22"), + VWCarDocs("Volkswagen Teramont X 2021-22"), + ], + VolkswagenCarSpecs(mass=2011, wheelbase=2.98), + ) + CADDY_MK3 = VolkswagenPQPlatformConfig( + "VOLKSWAGEN CADDY 3RD GEN", # Chassis 2K + [ + VWCarDocs("Volkswagen Caddy 2019"), + VWCarDocs("Volkswagen Caddy Maxi 2019"), + ], + VolkswagenCarSpecs(mass=1613, wheelbase=2.6, minSteerSpeed=21 * CV.KPH_TO_MS), + ) + CRAFTER_MK2 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN CRAFTER 2ND GEN", # Chassis SY/SZ + [ + VWCarDocs("Volkswagen Crafter 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("Volkswagen e-Crafter 2018-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("Volkswagen Grand California 2019-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("MAN TGE 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("MAN eTGE 2020-23", video_link="https://youtu.be/4100gLeabmo"), + ], + VolkswagenCarSpecs(mass=2100, wheelbase=3.64, minSteerSpeed=50 * CV.KPH_TO_MS), + ) + GOLF_MK7 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN GOLF 7TH GEN", # Chassis 5G/AU/BA/BE + [ + VWCarDocs("Volkswagen e-Golf 2014-20"), + VWCarDocs("Volkswagen Golf 2015-20", auto_resume=False), + VWCarDocs("Volkswagen Golf Alltrack 2015-19", auto_resume=False), + VWCarDocs("Volkswagen Golf GTD 2015-20"), + VWCarDocs("Volkswagen Golf GTE 2015-20"), + VWCarDocs("Volkswagen Golf GTI 2015-21", auto_resume=False), + VWCarDocs("Volkswagen Golf R 2015-19"), + VWCarDocs("Volkswagen Golf SportsVan 2015-20"), + ], + VolkswagenCarSpecs(mass=1397, wheelbase=2.62), + ) + JETTA_MK7 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN JETTA 7TH GEN", # Chassis BU + [ + VWCarDocs("Volkswagen Jetta 2018-24"), + VWCarDocs("Volkswagen Jetta GLI 2021-24"), + ], + VolkswagenCarSpecs(mass=1328, wheelbase=2.71), + ) + PASSAT_MK8 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN PASSAT 8TH GEN", # Chassis 3G + [ + VWCarDocs("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]), + VWCarDocs("Volkswagen Passat Alltrack 2015-22"), + VWCarDocs("Volkswagen Passat GTE 2015-22"), + ], + VolkswagenCarSpecs(mass=1551, wheelbase=2.79), + ) + PASSAT_NMS = VolkswagenPQPlatformConfig( + "VOLKSWAGEN PASSAT NMS", # Chassis A3 + [VWCarDocs("Volkswagen Passat NMS 2017-22")], + VolkswagenCarSpecs(mass=1503, wheelbase=2.80, minSteerSpeed=50*CV.KPH_TO_MS, minEnableSpeed=20*CV.KPH_TO_MS), + ) + POLO_MK6 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN POLO 6TH GEN", # Chassis AW + [ + VWCarDocs("Volkswagen Polo 2018-23", footnotes=[Footnote.VW_MQB_A0]), + VWCarDocs("Volkswagen Polo GTI 2018-23", footnotes=[Footnote.VW_MQB_A0]), + ], + VolkswagenCarSpecs(mass=1230, wheelbase=2.55), + ) + SHARAN_MK2 = VolkswagenPQPlatformConfig( + "VOLKSWAGEN SHARAN 2ND GEN", # Chassis 7N + [ + VWCarDocs("Volkswagen Sharan 2018-22"), + VWCarDocs("SEAT Alhambra 2018-20"), + ], + VolkswagenCarSpecs(mass=1639, wheelbase=2.92, minSteerSpeed=50*CV.KPH_TO_MS), + ) + TAOS_MK1 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN TAOS 1ST GEN", # Chassis B2 + [VWCarDocs("Volkswagen Taos 2022-23")], + VolkswagenCarSpecs(mass=1498, wheelbase=2.69), + ) + TCROSS_MK1 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN T-CROSS 1ST GEN", # Chassis C1 + [VWCarDocs("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0])], + VolkswagenCarSpecs(mass=1150, wheelbase=2.60), + ) + TIGUAN_MK2 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN TIGUAN 2ND GEN", # Chassis AD/BW + [ + VWCarDocs("Volkswagen Tiguan 2018-24"), + VWCarDocs("Volkswagen Tiguan eHybrid 2021-23"), + ], + VolkswagenCarSpecs(mass=1715, wheelbase=2.74), + ) + TOURAN_MK2 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN TOURAN 2ND GEN", # Chassis 1T + [VWCarDocs("Volkswagen Touran 2016-23")], + VolkswagenCarSpecs(mass=1516, wheelbase=2.79), + ) + TRANSPORTER_T61 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN TRANSPORTER T6.1", # Chassis 7H/7L + [ + VWCarDocs("Volkswagen Caravelle 2020"), + VWCarDocs("Volkswagen California 2021-23"), + ], + VolkswagenCarSpecs(mass=1926, wheelbase=3.00, minSteerSpeed=14.0), + ) + TROC_MK1 = VolkswagenMQBPlatformConfig( + "VOLKSWAGEN T-ROC 1ST GEN", # Chassis A1 + [VWCarDocs("Volkswagen T-Roc 2018-22", footnotes=[Footnote.VW_MQB_A0])], + VolkswagenCarSpecs(mass=1413, wheelbase=2.63), + ) + AUDI_A3_MK3 = VolkswagenMQBPlatformConfig( + "AUDI A3 3RD GEN", # Chassis 8V/FF + [ + VWCarDocs("Audi A3 2014-19"), + VWCarDocs("Audi A3 Sportback e-tron 2017-18"), + VWCarDocs("Audi RS3 2018"), + VWCarDocs("Audi S3 2015-17"), + ], + VolkswagenCarSpecs(mass=1335, wheelbase=2.61), + ) + AUDI_Q2_MK1 = VolkswagenMQBPlatformConfig( + "AUDI Q2 1ST GEN", # Chassis GA + [VWCarDocs("Audi Q2 2018")], + VolkswagenCarSpecs(mass=1205, wheelbase=2.61), + ) + AUDI_Q3_MK2 = VolkswagenMQBPlatformConfig( + "AUDI Q3 2ND GEN", # Chassis 8U/F3/FS + [VWCarDocs("Audi Q3 2019-23")], + VolkswagenCarSpecs(mass=1623, wheelbase=2.68), + ) + SEAT_ATECA_MK1 = VolkswagenMQBPlatformConfig( + "SEAT ATECA 1ST GEN", # Chassis 5F + [VWCarDocs("SEAT Ateca 2018")], + VolkswagenCarSpecs(mass=1900, wheelbase=2.64), + ) + SEAT_LEON_MK3 = VolkswagenMQBPlatformConfig( + "SEAT LEON 3RD GEN", # Chassis 5F + [VWCarDocs("SEAT Leon 2014-20")], + VolkswagenCarSpecs(mass=1227, wheelbase=2.64), + ) + SKODA_FABIA_MK4 = VolkswagenMQBPlatformConfig( + "SKODA FABIA 4TH GEN", # Chassis PJ + [VWCarDocs("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0])], + VolkswagenCarSpecs(mass=1266, wheelbase=2.56), + ) + SKODA_KAMIQ_MK1 = VolkswagenMQBPlatformConfig( + "SKODA KAMIQ 1ST GEN", # Chassis NW + [VWCarDocs("Škoda Kamiq 2021-23", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ])], + VolkswagenCarSpecs(mass=1265, wheelbase=2.66), + ) + SKODA_KAROQ_MK1 = VolkswagenMQBPlatformConfig( + "SKODA KAROQ 1ST GEN", # Chassis NU + [VWCarDocs("Škoda Karoq 2019-23")], + VolkswagenCarSpecs(mass=1278, wheelbase=2.66), + ) + SKODA_KODIAQ_MK1 = VolkswagenMQBPlatformConfig( + "SKODA KODIAQ 1ST GEN", # Chassis NS + [VWCarDocs("Škoda Kodiaq 2017-23")], + VolkswagenCarSpecs(mass=1569, wheelbase=2.79), + ) + SKODA_OCTAVIA_MK3 = VolkswagenMQBPlatformConfig( + "SKODA OCTAVIA 3RD GEN", # Chassis NE + [ + VWCarDocs("Škoda Octavia 2015-19"), + VWCarDocs("Škoda Octavia RS 2016"), + ], + VolkswagenCarSpecs(mass=1388, wheelbase=2.68), + ) + SKODA_SCALA_MK1 = VolkswagenMQBPlatformConfig( + "SKODA SCALA 1ST GEN", # Chassis NW + [VWCarDocs("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0])], + VolkswagenCarSpecs(mass=1192, wheelbase=2.65), + ) + SKODA_SUPERB_MK3 = VolkswagenMQBPlatformConfig( + "SKODA SUPERB 3RD GEN", # Chassis 3V/NP + [VWCarDocs("Škoda Superb 2015-22")], + VolkswagenCarSpecs(mass=1505, wheelbase=2.84), + ) # All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars @@ -294,18 +394,27 @@ VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + VOLKSWAGEN_RX_OFFSET = 0x6a FW_QUERY_CONFIG = FwQueryConfig( - requests=[ + # TODO: add back whitelists after we gather enough data + requests=[request for bus, obd_multiplexing in [(1, True), (1, False), (0, False)] for request in [ Request( [VOLKSWAGEN_VERSION_REQUEST_MULTI], [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], + # whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], rx_offset=VOLKSWAGEN_RX_OFFSET, + bus=bus, + logging=(bus != 1 or not obd_multiplexing), + obd_multiplexing=obd_multiplexing, ), Request( [VOLKSWAGEN_VERSION_REQUEST_MULTI], [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.transmission], + # whitelist_ecus=[Ecu.engine, Ecu.transmission], + bus=bus, + logging=(bus != 1 or not obd_multiplexing), + obd_multiplexing=obd_multiplexing, ), - ], + ]], extra_ecus=[(Ecu.fwdCamera, 0x74f, None)], ) + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/debug/README.md b/selfdrive/debug/README.md new file mode 100644 index 0000000..83b8a99 --- /dev/null +++ b/selfdrive/debug/README.md @@ -0,0 +1,59 @@ +# debug scripts + +## [can_printer.py](can_printer.py) + +``` +usage: can_printer.py [-h] [--bus BUS] [--max_msg MAX_MSG] [--addr ADDR] + +simple CAN data viewer + +optional arguments: + -h, --help show this help message and exit + --bus BUS CAN bus to print out (default: 0) + --max_msg MAX_MSG max addr (default: None) + --addr ADDR +``` + +## [dump.py](dump.py) + +``` +usage: dump.py [-h] [--pipe] [--raw] [--json] [--dump-json] [--no-print] [--addr ADDR] [--values VALUES] [socket [socket ...]] + +Dump communication sockets. See cereal/services.py for a complete list of available sockets. + +positional arguments: + socket socket names to dump. defaults to all services defined in cereal + +optional arguments: + -h, --help show this help message and exit + --pipe + --raw + --json + --dump-json + --no-print + --addr ADDR + --values VALUES values to monitor (instead of entire event) +``` + +## [vw_mqb_config.py](vw_mqb_config.py) + +``` +usage: vw_mqb_config.py [-h] [--debug] {enable,show,disable} + +Shows Volkswagen EPS software and coding info, and enables or disables Heading Control +Assist (Lane Assist). Useful for enabling HCA on cars without factory Lane Assist that want +to use openpilot integrated at the CAN gateway (J533). + +positional arguments: + {enable,show,disable} + show or modify current EPS HCA config + +optional arguments: + -h, --help show this help message and exit + --debug enable ISO-TP/UDS stack debugging output + +This tool is meant to run directly on a vehicle-installed comma three, with +the openpilot/tmux processes stopped. It should also work on a separate PC with a USB- +attached comma panda. Vehicle ignition must be on. Recommend engine not be running when +making changes. Must turn ignition off and on again for any changes to take effect. +``` diff --git a/selfdrive/debug/__init__.py b/selfdrive/debug/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/debug/adb.sh b/selfdrive/debug/adb.sh new file mode 100644 index 0000000..919a82f --- /dev/null +++ b/selfdrive/debug/adb.sh @@ -0,0 +1,11 @@ +#!/usr/bin/bash +set -e + +PORT=5555 + +setprop service.adb.tcp.port $PORT +sudo systemctl start adbd + +IP=$(echo $SSH_CONNECTION | awk '{ print $3}') +echo "then, connect on your computer:" +echo "adb connect $IP:$PORT" diff --git a/selfdrive/debug/can_print_changes.py b/selfdrive/debug/can_print_changes.py new file mode 100644 index 0000000..97d60b2 --- /dev/null +++ b/selfdrive/debug/can_print_changes.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +import argparse +import binascii +import time +from collections import defaultdict + +import cereal.messaging as messaging +from openpilot.selfdrive.debug.can_table import can_table +from openpilot.tools.lib.logreader import LogIterable, LogReader + +RED = '\033[91m' +CLEAR = '\033[0m' + +def update(msgs, bus, dat, low_to_high, high_to_low, quiet=False): + for x in msgs: + if x.which() != 'can': + continue + + for y in x.can: + if y.src == bus: + dat[y.address] = y.dat + + i = int.from_bytes(y.dat, byteorder='big') + l_h = low_to_high[y.address] + h_l = high_to_low[y.address] + + change = None + if (i | l_h) != l_h: + low_to_high[y.address] = i | l_h + change = "+" + + if (~i | h_l) != h_l: + high_to_low[y.address] = ~i | h_l + change = "-" + + if change and not quiet: + print(f"{time.monotonic():.2f}\t{hex(y.address)} ({y.address})\t{change}{binascii.hexlify(y.dat)}") + + +def can_printer(bus=0, init_msgs=None, new_msgs=None, table=False): + logcan = messaging.sub_sock('can', timeout=10) + + dat = defaultdict(int) + low_to_high = defaultdict(int) + high_to_low = defaultdict(int) + + if init_msgs is not None: + update(init_msgs, bus, dat, low_to_high, high_to_low, quiet=True) + + low_to_high_init = low_to_high.copy() + high_to_low_init = high_to_low.copy() + + if new_msgs is not None: + update(new_msgs, bus, dat, low_to_high, high_to_low) + else: + # Live mode + print(f"Waiting for messages on bus {bus}") + try: + while 1: + can_recv = messaging.drain_sock(logcan) + update(can_recv, bus, dat, low_to_high, high_to_low) + time.sleep(0.02) + except KeyboardInterrupt: + pass + + print("\n\n") + tables = "" + for addr in sorted(dat.keys()): + init = low_to_high_init[addr] & high_to_low_init[addr] + now = low_to_high[addr] & high_to_low[addr] + d = now & ~init + if d == 0: + continue + b = d.to_bytes(len(dat[addr]), byteorder='big') + + byts = ''.join([(c if c == '0' else f'{RED}{c}{CLEAR}') for c in str(binascii.hexlify(b))[2:-1]]) + header = f"{hex(addr).ljust(6)}({str(addr).ljust(4)})" + print(header, byts) + tables += f"{header}\n" + tables += can_table(b) + "\n\n" + + if table: + print(tables) + +if __name__ == "__main__": + desc = """Collects messages and prints when a new bit transition is observed. + This is very useful to find signals based on user triggered actions, such as blinkers and seatbelt. + Leave the script running until no new transitions are seen, then perform the action.""" + parser = argparse.ArgumentParser(description=desc, + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0) + parser.add_argument("--table", action="store_true", help="Print a cabana-like table") + parser.add_argument("init", type=str, nargs='?', help="Route or segment to initialize with. Use empty quotes to compare against all zeros.") + parser.add_argument("comp", type=str, nargs='?', help="Route or segment to compare against init") + + args = parser.parse_args() + + init_lr: LogIterable | None = None + new_lr: LogIterable | None = None + + if args.init: + if args.init == '': + init_lr = [] + else: + init_lr = LogReader(args.init) + if args.comp: + new_lr = LogReader(args.comp) + + can_printer(args.bus, init_msgs=init_lr, new_msgs=new_lr, table=args.table) diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py new file mode 100755 index 0000000..2200089 --- /dev/null +++ b/selfdrive/debug/can_printer.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import argparse +import binascii +import time +from collections import defaultdict + +import cereal.messaging as messaging + + +def can_printer(bus, max_msg, addr, ascii_decode): + logcan = messaging.sub_sock('can', addr=addr) + + start = time.monotonic() + lp = time.monotonic() + msgs = defaultdict(list) + while 1: + can_recv = messaging.drain_sock(logcan, wait_for_one=True) + for x in can_recv: + for y in x.can: + if y.src == bus: + msgs[y.address].append(y.dat) + + if time.monotonic() - lp > 0.1: + dd = chr(27) + "[2J" + dd += f"{time.monotonic() - start:5.2f}\n" + for addr in sorted(msgs.keys()): + a = f"\"{msgs[addr][-1].decode('ascii', 'backslashreplace')}\"" if ascii_decode else "" + x = binascii.hexlify(msgs[addr][-1]).decode('ascii') + freq = len(msgs[addr]) / (time.monotonic() - start) + if max_msg is None or addr < max_msg: + dd += "%04X(%4d)(%6d)(%3dHz) %s %s\n" % (addr, addr, len(msgs[addr]), freq, x.ljust(20), a) + print(dd) + lp = time.monotonic() + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="simple CAN data viewer", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0) + parser.add_argument("--max_msg", type=int, help="max addr") + parser.add_argument("--ascii", action='store_true', help="decode as ascii") + parser.add_argument("--addr", default="127.0.0.1") + + args = parser.parse_args() + can_printer(args.bus, args.max_msg, args.addr, args.ascii) diff --git a/selfdrive/debug/can_table.py b/selfdrive/debug/can_table.py new file mode 100644 index 0000000..11d070e --- /dev/null +++ b/selfdrive/debug/can_table.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import argparse +import pandas as pd + +import cereal.messaging as messaging + + +def can_table(dat): + rows = [] + for b in dat: + r = list(bin(b).lstrip('0b').zfill(8)) + r += [hex(b)] + rows.append(r) + + df = pd.DataFrame(data=rows) + df.columns = [str(n) for n in range(7, -1, -1)] + [' '] + table = df.to_markdown(tablefmt='grid') + return table + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Cabana-like table of bits for your terminal", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("addr", type=str, nargs=1) + parser.add_argument("bus", type=int, default=0, nargs='?') + + args = parser.parse_args() + + addr = int(args.addr[0], 0) + can = messaging.sub_sock('can', conflate=False, timeout=None) + + print(f"waiting for {hex(addr)} ({addr}) on bus {args.bus}...") + + latest = None + while True: + for msg in messaging.drain_sock(can, wait_for_one=True): + for m in msg.can: + if m.address == addr and m.src == args.bus: + latest = m + + if latest is None: + continue + + table = can_table(latest.dat) + print(f"\n\n{hex(addr)} ({addr}) on bus {args.bus}\n{table}") diff --git a/selfdrive/debug/check_can_parser_performance.py b/selfdrive/debug/check_can_parser_performance.py new file mode 100644 index 0000000..c4b688c --- /dev/null +++ b/selfdrive/debug/check_can_parser_performance.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import numpy as np +import time +from tqdm import tqdm + +from cereal import car +from openpilot.selfdrive.car.tests.routes import CarTestRoute +from openpilot.selfdrive.car.tests.test_models import TestCarModelBase +from openpilot.tools.plotjuggler.juggle import DEMO_ROUTE + +N_RUNS = 10 + + +class CarModelTestCase(TestCarModelBase): + test_route = CarTestRoute(DEMO_ROUTE, None) + ci = False + + +if __name__ == '__main__': + # Get CAN messages and parsers + tm = CarModelTestCase() + tm.setUpClass() + tm.setUp() + + CC = car.CarControl.new_message() + ets = [] + for _ in tqdm(range(N_RUNS)): + start_t = time.process_time_ns() + for msg in tm.can_msgs: + for cp in tm.CI.can_parsers: + if cp is not None: + cp.update_strings((msg.as_builder().to_bytes(),)) + ets.append((time.process_time_ns() - start_t) * 1e-6) + + print(f'{len(tm.can_msgs)} CAN packets, {N_RUNS} runs') + print(f'{np.mean(ets):.2f} mean ms, {max(ets):.2f} max ms, {min(ets):.2f} min ms, {np.std(ets):.2f} std ms') + print(f'{np.mean(ets) / len(tm.can_msgs):.4f} mean ms / CAN packet') diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py new file mode 100755 index 0000000..1765aeb --- /dev/null +++ b/selfdrive/debug/check_freq.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +import argparse +import numpy as np +import time +from collections import defaultdict, deque +from collections.abc import MutableSequence + +import cereal.messaging as messaging + + +if __name__ == "__main__": + context = messaging.Context() + poller = messaging.Poller() + + parser = argparse.ArgumentParser() + parser.add_argument("socket", type=str, nargs='*', help="socket name") + args = parser.parse_args() + + socket_names = args.socket + sockets = {} + + rcv_times: defaultdict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) + valids: defaultdict[str, deque[bool]] = defaultdict(lambda: deque(maxlen=100)) + + t = time.monotonic() + for name in socket_names: + sock = messaging.sub_sock(name, poller=poller) + sockets[sock] = name + + prev_print = t + while True: + for socket in poller.poll(100): + msg = messaging.recv_one(socket) + if msg is None: + continue + + name = msg.which() + + t = time.monotonic() + rcv_times[name].append(msg.logMonoTime / 1e9) + valids[name].append(msg.valid) + + if t - prev_print > 1: + print() + for name in socket_names: + dts = np.diff(rcv_times[name]) + mean = np.mean(dts) + print(f"{name}: Freq {1.0 / mean:.2f} Hz, Min {np.min(dts) / mean * 100:.2f}%, Max {np.max(dts) / mean * 100:.2f}%, valid ", all(valids[name])) + + prev_print = t diff --git a/selfdrive/debug/check_lag.py b/selfdrive/debug/check_lag.py new file mode 100644 index 0000000..341ae79 --- /dev/null +++ b/selfdrive/debug/check_lag.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST + +TO_CHECK = ['carState'] + + +if __name__ == "__main__": + sm = messaging.SubMaster(TO_CHECK) + + prev_t: dict[str, float] = {} + + while True: + sm.update() + + for s in TO_CHECK: + if sm.updated[s]: + t = sm.logMonoTime[s] / 1e9 + + if s in prev_t: + expected = 1.0 / (SERVICE_LIST[s].frequency) + dt = t - prev_t[s] + if dt > 10 * expected: + print(t, s, dt) + + prev_t[s] = t diff --git a/selfdrive/debug/check_timings.py b/selfdrive/debug/check_timings.py new file mode 100644 index 0000000..3de6b8e --- /dev/null +++ b/selfdrive/debug/check_timings.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 + +import sys +import time +import numpy as np +from collections.abc import MutableSequence +from collections import defaultdict, deque + +import cereal.messaging as messaging + +socks = {s: messaging.sub_sock(s, conflate=False) for s in sys.argv[1:]} +ts: defaultdict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) + +if __name__ == "__main__": + while True: + print() + for s, sock in socks.items(): + msgs = messaging.drain_sock(sock) + for m in msgs: + ts[s].append(m.logMonoTime / 1e6) + + if len(ts[s]) > 2: + d = np.diff(ts[s]) + print(f"{s:25} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}") + time.sleep(1) diff --git a/selfdrive/debug/clear_dtc.py b/selfdrive/debug/clear_dtc.py new file mode 100644 index 0000000..dea2133 --- /dev/null +++ b/selfdrive/debug/clear_dtc.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import sys +import argparse +from subprocess import check_output, CalledProcessError +from panda import Panda +from panda.python.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE + +parser = argparse.ArgumentParser(description="clear DTC status") +parser.add_argument("addr", type=lambda x: int(x,0), nargs="?", default=0x7DF) # default is functional (broadcast) address +parser.add_argument("--bus", type=int, default=0) +parser.add_argument('--debug', action='store_true') +args = parser.parse_args() + +try: + check_output(["pidof", "boardd"]) + print("boardd is running, please kill openpilot before running this script! (aborted)") + sys.exit(1) +except CalledProcessError as e: + if e.returncode != 1: # 1 == no process found (boardd not running) + raise e + +panda = Panda() +panda.set_safety_mode(Panda.SAFETY_ELM327) +uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug) +print("extended diagnostic session ...") +try: + uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) +except MessageTimeoutError: + # functional address isn't properly handled so a timeout occurs + if args.addr != 0x7DF: + raise +print("clear diagnostic info ...") +try: + uds_client.clear_diagnostic_information(DTC_GROUP_TYPE.ALL) +except MessageTimeoutError: + # functional address isn't properly handled so a timeout occurs + if args.addr != 0x7DF: + pass +print("") +print("you may need to power cycle your vehicle now") diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py new file mode 100644 index 0000000..5942054 --- /dev/null +++ b/selfdrive/debug/count_events.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +import sys +import math +import datetime +from collections import Counter +from pprint import pprint +from typing import cast + +from cereal.services import SERVICE_LIST +from openpilot.tools.lib.logreader import LogReader, ReadMode + +if __name__ == "__main__": + cnt_events: Counter = Counter() + + cams = [s for s in SERVICE_LIST if s.endswith('CameraState')] + cnt_cameras = dict.fromkeys(cams, 0) + + events: list[tuple[float, set[str]]] = [] + alerts: list[tuple[float, str]] = [] + start_time = math.inf + end_time = -math.inf + ignition_off = None + for msg in LogReader(sys.argv[1], ReadMode.QLOG): + t = (msg.logMonoTime - start_time) / 1e9 + end_time = max(end_time, msg.logMonoTime) + start_time = min(start_time, msg.logMonoTime) + + if msg.which() == 'onroadEvents': + for e in msg.onroadEvents: + cnt_events[e.name] += 1 + + ae = {str(e.name) for e in msg.onroadEvents if e.name not in ('pedalPressed', 'steerOverride', 'gasPressedOverride')} + if len(events) == 0 or ae != events[-1][1]: + events.append((t, ae)) + + elif msg.which() == 'controlsState': + at = msg.controlsState.alertType + if "/override" not in at or "lanechange" in at.lower(): + if len(alerts) == 0 or alerts[-1][1] != at: + alerts.append((t, at)) + elif msg.which() == 'pandaStates': + if ignition_off is None: + ign = any(ps.ignitionLine or ps.ignitionCan for ps in msg.pandaStates) + if not ign: + ignition_off = msg.logMonoTime + break + elif msg.which() in cams: + cnt_cameras[msg.which()] += 1 + + duration = (end_time - start_time) / 1e9 + + print("Events") + pprint(cnt_events) + + print("\n") + print("Events") + for t, evt in events: + print(f"{t:8.2f} {evt}") + + print("\n") + print("Cameras") + for k, v in cnt_cameras.items(): + s = SERVICE_LIST[k] + expected_frames = int(s.frequency * duration / cast(float, s.decimation)) + print(" ", k.ljust(20), f"{v}, {v/expected_frames:.1%} of expected") + + print("\n") + print("Alerts") + for t, a in alerts: + print(f"{t:8.2f} {a}") + + print("\n") + if ignition_off is not None: + ignition_off = round((ignition_off - start_time) / 1e9, 2) + print("Ignition off at", ignition_off) + print("Route duration", datetime.timedelta(seconds=duration)) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py new file mode 100644 index 0000000..ec9d02e --- /dev/null +++ b/selfdrive/debug/cpu_usage_stat.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +# type: ignore +''' +System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs. + Features: + Use psutil library to sample cpu usage(avergage for all cores) of openpilot processes, at a rate of 5 samples/sec. + Do cpu usage statistics periodically, 5 seconds as a cycle. + Calculate the average cpu usage within this cycle. + Calculate minumium/maximum/accumulated_average cpu usage as long term inspections. + Monitor multiple processes simuteneously. + Sample usage: + root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd + ('Add monitored proc:', './boardd') + ('Add monitored proc:', 'python locationd/ubloxd.py') + boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96% + ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39% +''' +import psutil +import time +import os +import sys +import numpy as np +import argparse +import re +from collections import defaultdict + +from openpilot.selfdrive.manager.process_config import managed_processes + +# Do statistics every 5 seconds +PRINT_INTERVAL = 5 +SLEEP_INTERVAL = 0.2 + +monitored_proc_names = [ + # android procs + 'SurfaceFlinger', 'sensors.qcom' +] + list(managed_processes.keys()) + +cpu_time_names = ['user', 'system', 'children_user', 'children_system'] + +timer = getattr(time, 'monotonic', time.time) + + +def get_arg_parser(): + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("proc_names", nargs="?", default='', + help="Process names to be monitored, comma separated") + parser.add_argument("--list_all", action='store_true', + help="Show all running processes' cmdline") + parser.add_argument("--detailed_times", action='store_true', + help="show cpu time details (split by user, system, child user, child system)") + return parser + + +if __name__ == "__main__": + args = get_arg_parser().parse_args(sys.argv[1:]) + if args.list_all: + for p in psutil.process_iter(): + print('cmdline', p.cmdline(), 'name', p.name()) + sys.exit(0) + + if len(args.proc_names) > 0: + monitored_proc_names = args.proc_names.split(',') + monitored_procs = [] + stats = {} + for p in psutil.process_iter(): + if p == psutil.Process(): + continue + matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(fr'.*{pn}.*', l, re.M | re.I))) + if matched: + k = ' '.join(p.cmdline()) + print('Add monitored proc:', k) + stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None), + 'avg': defaultdict(float), 'last_cpu_times': None, 'last_sys_time': None} + stats[k]['last_sys_time'] = timer() + stats[k]['last_cpu_times'] = p.cpu_times() + monitored_procs.append(p) + i = 0 + interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL) + while True: + for p in monitored_procs: + k = ' '.join(p.cmdline()) + cur_sys_time = timer() + cur_cpu_times = p.cpu_times() + cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time']) + stats[k]['last_sys_time'] = cur_sys_time + stats[k]['last_cpu_times'] = cur_cpu_times + cpu_percent = 0 + for num, name in enumerate(cpu_time_names): + stats[k]['cpu_samples'][name].append(cpu_times[num]) + cpu_percent += cpu_times[num] + stats[k]['cpu_samples']['total'].append(cpu_percent) + time.sleep(SLEEP_INTERVAL) + i += 1 + if i % interval_int == 0: + l = [] + for k, stat in stats.items(): + if len(stat['cpu_samples']) <= 0: + continue + for name, samples in stat['cpu_samples'].items(): + samples = np.array(samples) + avg = samples.mean() + c = samples.size + min_cpu = np.amin(samples) + max_cpu = np.amax(samples) + if stat['min'][name] is None or min_cpu < stat['min'][name]: + stat['min'][name] = min_cpu + if stat['max'][name] is None or max_cpu > stat['max'][name]: + stat['max'][name] = max_cpu + stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i) + stat['cpu_samples'][name] = [] + + msg = f"avg: {stat['avg']['total']:.2%}, min: {stat['min']['total']:.2%}, max: {stat['max']['total']:.2%} {os.path.basename(k)}" + if args.detailed_times: + for stat_type in ['avg', 'min', 'max']: + msg += f"\n {stat_type}: {[(name + ':' + str(round(stat[stat_type][name] * 100, 2))) for name in cpu_time_names]}" + l.append((os.path.basename(k), stat['avg']['total'], msg)) + l.sort(key=lambda x: -x[1]) + for x in l: + print(x[2]) + print('avg sum: {:.2%} over {} samples {} seconds\n'.format( + sum(stat['avg']['total'] for k, stat in stats.items()), i, i * SLEEP_INTERVAL + )) diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py new file mode 100644 index 0000000..42561f7 --- /dev/null +++ b/selfdrive/debug/cycle_alerts.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +import time +import random + +from cereal import car, log +import cereal.messaging as messaging +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car.honda.interface import CarInterface +from openpilot.selfdrive.controls.lib.events import ET, Events +from openpilot.selfdrive.controls.lib.alertmanager import AlertManager +from openpilot.selfdrive.manager.process_config import managed_processes + +EventName = car.CarEvent.EventName + +def randperc() -> float: + return 100. * random.random() + +def cycle_alerts(duration=200, is_metric=False): + # all alerts + #alerts = list(EVENTS.keys()) + + # this plays each type of audible alert + alerts = [ + (EventName.buttonEnable, ET.ENABLE), + (EventName.buttonCancel, ET.USER_DISABLE), + (EventName.wrongGear, ET.NO_ENTRY), + + (EventName.locationdTemporaryError, ET.SOFT_DISABLE), + (EventName.paramsdTemporaryError, ET.SOFT_DISABLE), + (EventName.accFaulted, ET.IMMEDIATE_DISABLE), + + # DM sequence + (EventName.preDriverDistracted, ET.WARNING), + (EventName.promptDriverDistracted, ET.WARNING), + (EventName.driverDistracted, ET.WARNING), + ] + + # debug alerts + alerts = [ + #(EventName.highCpuUsage, ET.NO_ENTRY), + #(EventName.lowMemory, ET.PERMANENT), + #(EventName.overheat, ET.PERMANENT), + #(EventName.outOfSpace, ET.PERMANENT), + #(EventName.modeldLagging, ET.PERMANENT), + #(EventName.processNotRunning, ET.NO_ENTRY), + #(EventName.commIssue, ET.NO_ENTRY), + #(EventName.calibrationInvalid, ET.PERMANENT), + (EventName.cameraMalfunction, ET.PERMANENT), + (EventName.cameraFrameRate, ET.PERMANENT), + ] + + cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState'] + + CS = car.CarState.new_message() + CP = CarInterface.get_non_essential_params("HONDA CIVIC 2016") + sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', + 'managerState'] + cameras) + + pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState']) + + events = Events() + AM = AlertManager() + + frame = 0 + while True: + for alert, et in alerts: + events.clear() + events.add(alert) + + sm['deviceState'].freeSpacePercent = randperc() + sm['deviceState'].memoryUsagePercent = int(randperc()) + sm['deviceState'].cpuTempC = [randperc() for _ in range(3)] + sm['deviceState'].gpuTempC = [randperc() for _ in range(3)] + sm['deviceState'].cpuUsagePercent = [int(randperc()) for _ in range(8)] + sm['modelV2'].frameDropPerc = randperc() + + if random.random() > 0.25: + sm['modelV2'].velocity.x = [random.random(), ] + if random.random() > 0.25: + CS.vEgo = random.random() + + procs = [p.get_process_state_msg() for p in managed_processes.values()] + random.shuffle(procs) + for i in range(random.randint(0, 10)): + procs[i].shouldBeRunning = True + sm['managerState'].processes = procs + + sm['liveCalibration'].rpyCalib = [-1 * random.random() for _ in range(random.randint(0, 3))] + + for s in sm.data.keys(): + prob = 0.3 if s in cameras else 0.08 + sm.alive[s] = random.random() > prob + sm.valid[s] = random.random() > prob + sm.freq_ok[s] = random.random() > prob + + a = events.create_alerts([et, ], [CP, CS, sm, is_metric, 0]) + AM.add_many(frame, a) + alert = AM.process_alerts(frame, []) + print(alert) + for _ in range(duration): + dat = messaging.new_message() + dat.init('controlsState') + dat.controlsState.enabled = False + + if alert: + dat.controlsState.alertText1 = alert.alert_text_1 + dat.controlsState.alertText2 = alert.alert_text_2 + dat.controlsState.alertSize = alert.alert_size + dat.controlsState.alertStatus = alert.alert_status + dat.controlsState.alertBlinkingRate = alert.alert_rate + dat.controlsState.alertType = alert.alert_type + dat.controlsState.alertSound = alert.audible_alert + pm.send('controlsState', dat) + + dat = messaging.new_message() + dat.init('deviceState') + dat.deviceState.started = True + pm.send('deviceState', dat) + + dat = messaging.new_message('pandaStates', 1) + dat.pandaStates[0].ignitionLine = True + dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno + pm.send('pandaStates', dat) + + frame += 1 + time.sleep(DT_CTRL) + +if __name__ == '__main__': + cycle_alerts() diff --git a/selfdrive/debug/debug_fw_fingerprinting_offline.py b/selfdrive/debug/debug_fw_fingerprinting_offline.py new file mode 100644 index 0000000..d521ab2 --- /dev/null +++ b/selfdrive/debug/debug_fw_fingerprinting_offline.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +import argparse + +from openpilot.tools.lib.logreader import LogReader, ReadMode +from panda.python import uds + + +def main(route: str, addrs: list[int]): + """ + TODO: + - highlight TX vs RX clearly + - disambiguate sendcan and can (useful to know if something sent on sendcan made it to the bus on can->128) + - print as fixed width table, easier to read + """ + + lr = LogReader(route, default_mode=ReadMode.RLOG) + + start_mono_time = None + prev_mono_time = 0 + + # include rx addresses + addrs = addrs + [uds.get_rx_addr_for_tx_addr(addr) for addr in addrs] + + for msg in lr: + if msg.which() == 'can': + if start_mono_time is None: + start_mono_time = msg.logMonoTime + + if msg.which() in ("can", 'sendcan'): + for can in getattr(msg, msg.which()): + if can.address in addrs: + if msg.logMonoTime != prev_mono_time: + print() + prev_mono_time = msg.logMonoTime + print(f"{msg.logMonoTime} rxaddr={can.address}, bus={can.src}, {round((msg.logMonoTime - start_mono_time) * 1e-6, 2)} ms, " + + f"0x{can.dat.hex()}, {can.dat}, {len(can.dat)=}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='View back and forth ISO-TP communication between various ECUs given an address') + parser.add_argument('route', help='Route name') + parser.add_argument('addrs', nargs='*', help='List of tx address to view (0x7e0 for engine)') + args = parser.parse_args() + + addrs = [int(addr, base=16) if addr.startswith('0x') else int(addr) for addr in args.addrs] + main(args.route, addrs) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py new file mode 100755 index 0000000..787e9bc --- /dev/null +++ b/selfdrive/debug/dump.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +import sys +import argparse +import json +import codecs + +from hexdump import hexdump +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.tools.lib.live_logreader import raw_live_logreader + + +codecs.register_error("strict", codecs.backslashreplace_errors) + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description='Dump communication sockets. See cereal/services.py for a complete list of available sockets.') + parser.add_argument('--pipe', action='store_true') + parser.add_argument('--raw', action='store_true') + parser.add_argument('--json', action='store_true') + parser.add_argument('--dump-json', action='store_true') + parser.add_argument('--no-print', action='store_true') + parser.add_argument('--addr', default='127.0.0.1') + parser.add_argument('--values', help='values to monitor (instead of entire event)') + parser.add_argument("socket", type=str, nargs='*', default=list(SERVICE_LIST.keys()), help="socket names to dump. defaults to all services defined in cereal") + args = parser.parse_args() + + lr = raw_live_logreader(args.socket, args.addr) + + values = None + if args.values: + values = [s.strip().split(".") for s in args.values.split(",")] + + for msg in lr: + with log.Event.from_bytes(msg) as evt: + if not args.no_print: + if args.pipe: + sys.stdout.write(str(msg)) + sys.stdout.flush() + elif args.raw: + hexdump(msg) + elif args.json: + print(json.loads(msg)) + elif args.dump_json: + print(json.dumps(evt.to_dict())) + elif values: + print(f"logMonotime = {evt.logMonoTime}") + for value in values: + if hasattr(evt, value[0]): + item = evt + for key in value: + item = getattr(item, key) + print(f"{'.'.join(value)} = {item}") + print("") + else: + try: + print(evt) + except UnicodeDecodeError: + w = evt.which() + s = f"( logMonoTime {evt.logMonoTime} \n {w} = " + s += str(evt.__getattr__(w)) + s += f"\n valid = {evt.valid} )" + print(s) diff --git a/selfdrive/debug/dump_car_docs.py b/selfdrive/debug/dump_car_docs.py new file mode 100644 index 0000000..f09c602 --- /dev/null +++ b/selfdrive/debug/dump_car_docs.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 +import argparse +import pickle + +from openpilot.selfdrive.car.docs import get_all_car_docs + + +def dump_car_docs(path): + with open(path, 'wb') as f: + pickle.dump(get_all_car_docs(), f) + print(f'Dumping car info to {path}') + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--path", required=True) + args = parser.parse_args() + dump_car_docs(args.path) diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py new file mode 100755 index 0000000..9cbab0b --- /dev/null +++ b/selfdrive/debug/filter_log_message.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +import argparse +import json + +import cereal.messaging as messaging +from openpilot.tools.lib.logreader import LogReader + +LEVELS = { + "DEBUG": 10, + "INFO": 20, + "WARNING": 30, + "ERROR": 40, + "CRITICAL": 50, +} + +ANDROID_LOG_SOURCE = { + 0: "MAIN", + 1: "RADIO", + 2: "EVENTS", + 3: "SYSTEM", + 4: "CRASH", + 5: "KERNEL", +} + + +def print_logmessage(t, msg, min_level): + try: + log = json.loads(msg) + if log['levelnum'] >= min_level: + print(f"[{t / 1e9:.6f}] {log['filename']}:{log.get('lineno', '')} - {log.get('funcname', '')}: {log['msg']}") + if 'exc_info' in log: + print(log['exc_info']) + except json.decoder.JSONDecodeError: + print(f"[{t / 1e9:.6f}] decode error: {msg}") + + +def print_androidlog(t, msg): + source = ANDROID_LOG_SOURCE[msg.id] + try: + m = json.loads(msg.message)['MESSAGE'] + except Exception: + m = msg.message + + print(f"[{t / 1e9:.6f}] {source} {msg.pid} {msg.tag} - {m}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('--absolute', action='store_true') + parser.add_argument('--level', default='DEBUG') + parser.add_argument('--addr', default='127.0.0.1') + parser.add_argument("route", type=str, nargs='*', help="route name + segment number for offline usage") + args = parser.parse_args() + + min_level = LEVELS[args.level] + + if args.route: + st = None if not args.absolute else 0 + for route in args.route: + lr = LogReader(route, sort_by_time=True) + for m in lr: + if st is None: + st = m.logMonoTime + if m.which() == 'logMessage': + print_logmessage(m.logMonoTime-st, m.logMessage, min_level) + elif m.which() == 'errorLogMessage': + print_logmessage(m.logMonoTime-st, m.errorLogMessage, min_level) + elif m.which() == 'androidLog': + print_androidlog(m.logMonoTime-st, m.androidLog) + else: + sm = messaging.SubMaster(['logMessage', 'androidLog'], addr=args.addr) + while True: + sm.update() + + if sm.updated['logMessage']: + print_logmessage(sm.logMonoTime['logMessage'], sm['logMessage'], min_level) + + if sm.updated['androidLog']: + print_androidlog(sm.logMonoTime['androidLog'], sm['androidLog']) diff --git a/selfdrive/debug/fingerprint_from_route.py b/selfdrive/debug/fingerprint_from_route.py new file mode 100644 index 0000000..68308bb --- /dev/null +++ b/selfdrive/debug/fingerprint_from_route.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import sys +from openpilot.tools.lib.logreader import LogReader, ReadMode + + +def get_fingerprint(lr): + # TODO: make this a nice tool for car ports. should also work with qlogs for FW + + fw = None + msgs = {} + for msg in lr: + if msg.which() == 'carParams': + fw = msg.carParams.carFw + elif msg.which() == 'can': + for c in msg.can: + # read also msgs sent by EON on CAN bus 0x80 and filter out the + # addr with more than 11 bits + if c.src % 0x80 == 0 and c.address < 0x800 and c.address not in (0x7df, 0x7e0, 0x7e8): + msgs[c.address] = len(c.dat) + + # show CAN fingerprint + fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) + print(f"\nfound {len(msgs)} messages. CAN fingerprint:\n") + print(fingerprint) + + # TODO: also print the fw fingerprint merged with the existing ones + # show FW fingerprint + print("\nFW fingerprint:\n") + for f in fw: + print(f" (Ecu.{f.ecu}, {hex(f.address)}, {None if f.subAddress == 0 else f.subAddress}): [") + print(f" {f.fwVersion},") + print(" ],") + print() + + +if __name__ == "__main__": + if len(sys.argv) < 2: + print("Usage: ./fingerprint_from_route.py ") + sys.exit(1) + + lr = LogReader(sys.argv[1], ReadMode.QLOG) + get_fingerprint(lr) diff --git a/selfdrive/debug/format_fingerprints.py b/selfdrive/debug/format_fingerprints.py new file mode 100755 index 0000000..2a5e4e6 --- /dev/null +++ b/selfdrive/debug/format_fingerprints.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +import jinja2 +import os + +from cereal import car +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.car.interfaces import get_interface_attr + +Ecu = car.CarParams.Ecu + +CARS = get_interface_attr('CAR') +FW_VERSIONS = get_interface_attr('FW_VERSIONS') +FINGERPRINTS = get_interface_attr('FINGERPRINTS') +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +FINGERPRINTS_PY_TEMPLATE = jinja2.Template(""" +{%- if FINGERPRINTS[brand] %} +# ruff: noqa: E501 +{% endif %} +{% if FW_VERSIONS[brand] %} +from cereal import car +{% endif %} +from openpilot.selfdrive.car.{{brand}}.values import CAR +{% if FW_VERSIONS[brand] %} + +Ecu = car.CarParams.Ecu +{% endif %} +{% if comments +%} +{{ comments | join() }} +{% endif %} +{% if FINGERPRINTS[brand] %} + +FINGERPRINTS = { +{% for car, fingerprints in FINGERPRINTS[brand].items() %} + CAR.{{car.name}}: [{ +{% for fingerprint in fingerprints %} +{% if not loop.first %} + {{ "{" }} +{% endif %} + {% for key, value in fingerprint.items() %}{{key}}: {{value}}{% if not loop.last %}, {% endif %}{% endfor %} + + }{% if loop.last %}]{% endif %}, +{% endfor %} +{% endfor %} +} +{% endif %} + +FW_VERSIONS{% if not FW_VERSIONS[brand] %}: dict[str, dict[tuple, list[bytes]]]{% endif %} = { +{% for car, _ in FW_VERSIONS[brand].items() %} + CAR.{{car.name}}: { +{% for key, fw_versions in FW_VERSIONS[brand][car].items() %} + (Ecu.{{ECU_NAME[key[0]]}}, 0x{{"%0x" | format(key[1] | int)}}, \ +{% if key[2] %}0x{{"%0x" | format(key[2] | int)}}{% else %}{{key[2]}}{% endif %}): [ + {% for fw_version in (fw_versions + extra_fw_versions.get(car, {}).get(key, [])) | unique | sort %} + {{fw_version}}, + {% endfor %} + ], +{% endfor %} + }, +{% endfor %} +} + +""", trim_blocks=True) + + +def format_brand_fw_versions(brand, extra_fw_versions: None | dict[str, dict[tuple, list[bytes]]] = None): + extra_fw_versions = extra_fw_versions or {} + + fingerprints_file = os.path.join(BASEDIR, f"selfdrive/car/{brand}/fingerprints.py") + with open(fingerprints_file) as f: + comments = [line for line in f.readlines() if line.startswith("#") and "noqa" not in line] + + with open(fingerprints_file, "w") as f: + f.write(FINGERPRINTS_PY_TEMPLATE.render(brand=brand, comments=comments, ECU_NAME=ECU_NAME, + FINGERPRINTS=FINGERPRINTS, FW_VERSIONS=FW_VERSIONS, + extra_fw_versions=extra_fw_versions)) + + +if __name__ == "__main__": + for brand in FW_VERSIONS.keys(): + format_brand_fw_versions(brand) diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py new file mode 100755 index 0000000..f7f7a16 --- /dev/null +++ b/selfdrive/debug/get_fingerprint.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 + +# simple script to get a vehicle fingerprint. + +# Instructions: +# - connect to a Panda +# - run selfdrive/boardd/boardd +# - launching this script +# Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint +# - since some messages are published at low frequency, keep this script running for at least 30s, +# until all messages are received at least once + +import cereal.messaging as messaging + +logcan = messaging.sub_sock('can') +msgs = {} +while True: + lc = messaging.recv_sock(logcan, True) + if lc is None: + continue + + for c in lc.can: + # read also msgs sent by EON on CAN bus 0x80 and filter out the + # addr with more than 11 bits + if c.src % 0x80 == 0 and c.address < 0x800 and c.address not in (0x7df, 0x7e0, 0x7e8): + msgs[c.address] = len(c.dat) + + fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) + + print(f"number of messages {len(msgs)}:") + print(f"fingerprint {fingerprint}") diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py new file mode 100755 index 0000000..e5cae0d --- /dev/null +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +"""Some Hyundai radars can be reconfigured to output (debug) radar points on bus 1. +Reconfiguration is done over UDS by reading/writing to 0x0142 using the Read/Write Data By Identifier +endpoints (0x22 & 0x2E). This script checks your radar firmware version against a list of known +firmware versions. If you want to try on a new radar make sure to note the default config value +in case it's different from the other radars and you need to revert the changes. + +After changing the config the car should not show any faults when openpilot is not running. +These config changes are persistent across car reboots. You need to run this script again +to go back to the default values. + +USE AT YOUR OWN RISK! Safety features, like AEB and FCW, might be affected by these changes.""" + +import sys +import argparse +from typing import NamedTuple +from subprocess import check_output, CalledProcessError + +from panda.python import Panda +from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE + +class ConfigValues(NamedTuple): + default_config: bytes + tracks_enabled: bytes + +# If your radar supports changing data identifier 0x0142 as well make a PR to +# this file to add your firmware version. Make sure to post a drive as proof! +# NOTE: these firmware versions do not match what openpilot uses +# because this script uses a different diagnostic session type +SUPPORTED_FW_VERSIONS = { + # 2020 SONATA + b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + b"DN8_ SCC F-CUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2021 SONATA HYBRID + b"DNhe SCC FHCUP 1.00 1.00 99110-L5000\x19\x04&\x13' ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2020 PALISADE + b"LX2_ SCC FHCUP 1.00 1.04 99110-S8100\x19\x05\x02\x16V ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2022 PALISADE + b"LX2_ SCC FHCUP 1.00 1.00 99110-S8110!\x04\x05\x17\x01 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2020 SANTA FE + b"TM__ SCC F-CUP 1.00 1.03 99110-S2000\x19\x050\x13' ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2020 GENESIS G70 + b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2019 SANTA FE + b"TM__ SCC F-CUP 1.00 1.00 99110-S1210\x19\x01%\x168 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + b"TM__ SCC F-CUP 1.00 1.02 99110-S2000\x18\x07\x08\x18W ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2021 K5 HEV + b"DLhe SCC FHCUP 1.00 1.02 99110-L7000 \x01 \x102 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), +} + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='configure radar to output points (or reset to default)') + parser.add_argument('--default', action="store_true", default=False, help='reset to default configuration (default: false)') + parser.add_argument('--debug', action="store_true", default=False, help='enable debug output (default: false)') + parser.add_argument('--bus', type=int, default=0, help='can bus to use (default: 0)') + args = parser.parse_args() + + try: + check_output(["pidof", "boardd"]) + print("boardd is running, please kill openpilot before running this script! (aborted)") + sys.exit(1) + except CalledProcessError as e: + if e.returncode != 1: # 1 == no process found (boardd not running) + raise e + + confirm = input("power on the vehicle keeping the engine off (press start button twice) then type OK to continue: ").upper().strip() + if confirm != "OK": + print("\nyou didn't type 'OK! (aborted)") + sys.exit(0) + + panda = Panda() + panda.set_safety_mode(Panda.SAFETY_ELM327) + uds_client = UdsClient(panda, 0x7D0, bus=args.bus, debug=args.debug) + + print("\n[START DIAGNOSTIC SESSION]") + session_type : SESSION_TYPE = 0x07 # type: ignore + uds_client.diagnostic_session_control(session_type) + + print("[HARDWARE/SOFTWARE VERSION]") + fw_version_data_id : DATA_IDENTIFIER_TYPE = 0xf100 # type: ignore + fw_version = uds_client.read_data_by_identifier(fw_version_data_id) + print(fw_version) + if fw_version not in SUPPORTED_FW_VERSIONS.keys(): + print("radar not supported! (aborted)") + sys.exit(1) + + print("[GET CONFIGURATION]") + config_data_id : DATA_IDENTIFIER_TYPE = 0x0142 # type: ignore + current_config = uds_client.read_data_by_identifier(config_data_id) + config_values = SUPPORTED_FW_VERSIONS[fw_version] + new_config = config_values.default_config if args.default else config_values.tracks_enabled + print(f"current config: 0x{current_config.hex()}") + if current_config != new_config: + print("[CHANGE CONFIGURATION]") + print(f"new config: 0x{new_config.hex()}") + uds_client.write_data_by_identifier(config_data_id, new_config) + if not args.default and current_config != SUPPORTED_FW_VERSIONS[fw_version].default_config: + print("\ncurrent config does not match expected default! (aborted)") + sys.exit(1) + + print("[DONE]") + print("\nrestart your vehicle and ensure there are no faults") + if not args.default: + print("you can run this script again with --default to go back to the original (factory) settings") + else: + print("[DONE]") + print("\ncurrent config is already the desired configuration") + sys.exit(0) diff --git a/selfdrive/debug/internal/__init__.py b/selfdrive/debug/internal/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/debug/internal/fuzz_fw_fingerprint.py b/selfdrive/debug/internal/fuzz_fw_fingerprint.py new file mode 100644 index 0000000..aedb3ad --- /dev/null +++ b/selfdrive/debug/internal/fuzz_fw_fingerprint.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# type: ignore +import random +from collections import defaultdict + +from tqdm import tqdm + +from openpilot.selfdrive.car.fw_versions import match_fw_to_car_fuzzy +from openpilot.selfdrive.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS +from openpilot.selfdrive.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS +from openpilot.selfdrive.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS +from openpilot.selfdrive.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS + + +FWS = {} +FWS.update(TOYOTA_FW_VERSIONS) +FWS.update(HONDA_FW_VERSIONS) +FWS.update(HYUNDAI_FW_VERSIONS) +FWS.update(VW_FW_VERSIONS) + +if __name__ == "__main__": + total = 0 + match = 0 + wrong_match = 0 + confusions = defaultdict(set) + + for _ in tqdm(range(1000)): + for candidate, fws in FWS.items(): + fw_dict = {} + for (_, addr, subaddr), fw_list in fws.items(): + fw_dict[(addr, subaddr)] = [random.choice(fw_list)] + + matches = match_fw_to_car_fuzzy(fw_dict, log=False, exclude=candidate) + + total += 1 + if len(matches) == 1: + if list(matches)[0] == candidate: + match += 1 + else: + confusions[candidate] |= matches + wrong_match += 1 + + print() + for candidate, wrong_matches in sorted(confusions.items()): + print(candidate, wrong_matches) + + print() + print(f"Total fuzz cases: {total}") + print(f"Correct matches: {match}") + print(f"Wrong matches: {wrong_match}") + + diff --git a/selfdrive/debug/internal/measure_modeld_packet_drop.py b/selfdrive/debug/internal/measure_modeld_packet_drop.py new file mode 100644 index 0000000..9814942 --- /dev/null +++ b/selfdrive/debug/internal/measure_modeld_packet_drop.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import cereal.messaging as messaging + +if __name__ == "__main__": + modeld_sock = messaging.sub_sock("modelV2") + + last_frame_id = None + start_t: int | None = None + frame_cnt = 0 + dropped = 0 + + while True: + m = messaging.recv_one(modeld_sock) + if m is None: + continue + + frame_id = m.modelV2.frameId + t = m.logMonoTime / 1e9 + frame_cnt += 1 + + if start_t is None: + start_t = t + last_frame_id = frame_id + continue + + d_frame = frame_id - last_frame_id + dropped += d_frame - 1 + + expected_num_frames = int((t - start_t) * 20) + frame_drop = 100 * (1 - (expected_num_frames / frame_cnt)) + print(f"Num dropped {dropped}, Drop compared to 20Hz: {frame_drop:.2f}%") + + last_frame_id = frame_id diff --git a/selfdrive/debug/internal/measure_torque_time_to_max.py b/selfdrive/debug/internal/measure_torque_time_to_max.py new file mode 100644 index 0000000..ef3152b --- /dev/null +++ b/selfdrive/debug/internal/measure_torque_time_to_max.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +# type: ignore + +import os +import argparse +import struct +from collections import deque +from statistics import mean + +from cereal import log +import cereal.messaging as messaging + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description='Sniff a communication socket') + parser.add_argument('--addr', default='127.0.0.1') + args = parser.parse_args() + + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + poller = messaging.Poller() + messaging.sub_sock('can', poller, addr=args.addr) + + active = 0 + start_t = 0 + start_v = 0 + max_v = 0 + max_t = 0 + window = deque(maxlen=10) + avg = 0 + while 1: + polld = poller.poll(1000) + for sock in polld: + msg = sock.receive() + with log.Event.from_bytes(msg) as log_evt: + evt = log_evt + + for item in evt.can: + if item.address == 0xe4 and item.src == 128: + torque_req = struct.unpack('!h', item.dat[0:2])[0] + # print(torque_req) + active = abs(torque_req) > 0 + if abs(torque_req) < 100: + if max_v > 5: + print(f'{start_v} -> {max_v} = {round(max_v - start_v, 2)} over {round(max_t - start_t, 2)}s') + start_t = evt.logMonoTime / 1e9 + start_v = avg + max_t = 0 + max_v = 0 + if item.address == 0x1ab and item.src == 0: + motor_torque = ((item.dat[0] & 0x3) << 8) + item.dat[1] + window.append(motor_torque) + avg = mean(window) + #print(f'{evt.logMonoTime}: {avg}') + if active and avg > max_v + 0.5: + max_v = avg + max_t = evt.logMonoTime / 1e9 diff --git a/selfdrive/debug/internal/qlog_size.py b/selfdrive/debug/internal/qlog_size.py new file mode 100644 index 0000000..b51cb3a --- /dev/null +++ b/selfdrive/debug/internal/qlog_size.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import argparse +import bz2 +from collections import defaultdict + +import matplotlib.pyplot as plt + +from cereal.services import SERVICE_LIST +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.route import Route + +MIN_SIZE = 0.5 # Percent size of total to show as separate entry + + +def make_pie(msgs, typ): + compressed_length_by_type = {k: len(bz2.compress(b"".join(v))) for k, v in msgs.items()} + + total = sum(compressed_length_by_type.values()) + + sizes = sorted(compressed_length_by_type.items(), key=lambda kv: kv[1]) + + print(f"{typ} - Total {total / 1024:.2f} kB") + for (name, sz) in sizes: + print(f"{name} - {sz / 1024:.2f} kB") + print() + + sizes_large = [(k, sz) for (k, sz) in sizes if sz >= total * MIN_SIZE / 100] + sizes_large += [('other', sum(sz for (_, sz) in sizes if sz < total * MIN_SIZE / 100))] + + labels, sizes = zip(*sizes_large, strict=True) + + plt.figure() + plt.title(f"{typ}") + plt.pie(sizes, labels=labels, autopct='%1.1f%%') + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Check qlog size based on a rlog') + parser.add_argument('route', help='route to use') + parser.add_argument('segment', type=int, help='segment number to use') + args = parser.parse_args() + + r = Route(args.route) + rlog = r.log_paths()[args.segment] + msgs = list(LogReader(rlog)) + + msgs_by_type = defaultdict(list) + for m in msgs: + msgs_by_type[m.which()].append(m.as_builder().to_bytes()) + + qlog_by_type = defaultdict(list) + for name, service in SERVICE_LIST.items(): + if service.decimation is None: + continue + + for i, msg in enumerate(msgs_by_type[name]): + if i % service.decimation == 0: + qlog_by_type[name].append(msg) + + make_pie(msgs_by_type, 'rlog') + make_pie(qlog_by_type, 'qlog') + plt.show() diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py new file mode 100644 index 0000000..8549b92 --- /dev/null +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +import argparse +import capnp +from collections import defaultdict + +from cereal.messaging import SubMaster +from openpilot.common.numpy_fast import mean + +def cputime_total(ct): + return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq + + +def cputime_busy(ct): + return ct.user + ct.nice + ct.system + ct.irq + ct.softirq + + +def proc_cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +def proc_name(proc): + name = proc.name + if len(proc.cmdline): + name = proc.cmdline[0] + if len(proc.exe): + name = proc.exe + " - " + name + + return name + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('--mem', action='store_true') + parser.add_argument('--cpu', action='store_true') + args = parser.parse_args() + + sm = SubMaster(['deviceState', 'procLog']) + + last_temp = 0.0 + last_mem = 0.0 + total_times = [0.]*8 + busy_times = [0.]*8 + + prev_proclog: capnp._DynamicStructReader | None = None + prev_proclog_t: int | None = None + + while True: + sm.update() + + if sm.updated['deviceState']: + t = sm['deviceState'] + last_temp = mean(t.cpuTempC) + last_mem = t.memoryUsagePercent + + if sm.updated['procLog']: + m = sm['procLog'] + + cores = [0.]*8 + total_times_new = [0.]*8 + busy_times_new = [0.]*8 + + for c in m.cpuTimes: + n = c.cpuNum + total_times_new[n] = cputime_total(c) + busy_times_new[n] = cputime_busy(c) + + for n in range(8): + t_busy = busy_times_new[n] - busy_times[n] + t_total = total_times_new[n] - total_times[n] + cores[n] = t_busy / t_total + + total_times = total_times_new[:] + busy_times = busy_times_new[:] + + print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C") + + if args.cpu and prev_proclog is not None and prev_proclog_t is not None: + procs: dict[str, float] = defaultdict(float) + dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9 + for proc in m.procs: + try: + name = proc_name(proc) + prev_proc = [p for p in prev_proclog.procs if proc.pid == p.pid][0] + cpu_time = proc_cputime_total(proc) - proc_cputime_total(prev_proc) + cpu_usage = cpu_time / dt * 100. + procs[name] += cpu_usage + except IndexError: + pass + + print("Top CPU usage:") + for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]: + print(f"{k.rjust(70)} {v:.2f} %") + print() + + if args.mem: + mems = {} + for proc in m.procs: + name = proc_name(proc) + mems[name] = float(proc.memRss) / 1e6 + print("Top memory usage:") + for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]: + print(f"{k.rjust(70)} {v:.2f} MB") + print() + + prev_proclog = m + prev_proclog_t = sm.logMonoTime['procLog'] diff --git a/selfdrive/debug/print_docs_diff.py b/selfdrive/debug/print_docs_diff.py new file mode 100644 index 0000000..7ef89a6 --- /dev/null +++ b/selfdrive/debug/print_docs_diff.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python3 +import argparse +from collections import defaultdict +import difflib +import pickle + +from openpilot.selfdrive.car.docs import get_all_car_docs +from openpilot.selfdrive.car.docs_definitions import Column + +FOOTNOTE_TAG = "{}" +STAR_ICON = '' +VIDEO_ICON = '' + \ + '' +COLUMNS = "|" + "|".join([column.value for column in Column]) + "|" +COLUMN_HEADER = "|---|---|---|{}|".format("|".join([":---:"] * (len(Column) - 3))) +ARROW_SYMBOL = "➡️" + + +def load_base_car_docs(path): + with open(path, "rb") as f: + return pickle.load(f) + + +def match_cars(base_cars, new_cars): + changes = [] + additions = [] + for new in new_cars: + # Addition if no close matches or close match already used + # Change if close match and not already used + matches = difflib.get_close_matches(new.name, [b.name for b in base_cars], cutoff=0.) + if not len(matches) or matches[0] in [c[1].name for c in changes]: + additions.append(new) + else: + changes.append((new, next(car for car in base_cars if car.name == matches[0]))) + + # Removal if base car not in changes + removals = [b for b in base_cars if b.name not in [c[1].name for c in changes]] + return changes, additions, removals + + +def build_column_diff(base_car, new_car): + row_builder = [] + for column in Column: + base_column = base_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) + new_column = new_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) + + if base_column != new_column: + row_builder.append(f"{base_column} {ARROW_SYMBOL} {new_column}") + else: + row_builder.append(new_column) + + return format_row(row_builder) + + +def format_row(builder): + return "|" + "|".join(builder) + "|" + + +def print_car_docs_diff(path): + base_car_docs = defaultdict(list) + new_car_docs = defaultdict(list) + + for car in load_base_car_docs(path): + base_car_docs[car.car_fingerprint].append(car) + for car in get_all_car_docs(): + new_car_docs[car.car_fingerprint].append(car) + + # Add new platforms to base cars so we can detect additions and removals in one pass + base_car_docs.update({car: [] for car in new_car_docs if car not in base_car_docs}) + + changes = defaultdict(list) + for base_car_model, base_cars in base_car_docs.items(): + # Match car info changes, and get additions and removals + new_cars = new_car_docs[base_car_model] + car_changes, car_additions, car_removals = match_cars(base_cars, new_cars) + + # Removals + for car_docs in car_removals: + changes["removals"].append(format_row([car_docs.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) + + # Additions + for car_docs in car_additions: + changes["additions"].append(format_row([car_docs.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) + + for new_car, base_car in car_changes: + # Column changes + row_diff = build_column_diff(base_car, new_car) + if ARROW_SYMBOL in row_diff: + changes["column"].append(row_diff) + + # Detail sentence changes + if base_car.detail_sentence != new_car.detail_sentence: + changes["detail"].append(f"- Sentence for {base_car.name} changed!\n" + + " ```diff\n" + + f" - {base_car.detail_sentence}\n" + + f" + {new_car.detail_sentence}\n" + + " ```") + + # Print diff + if any(len(c) for c in changes.values()): + markdown_builder = ["### ⚠️ This PR makes changes to [CARS.md](../blob/master/docs/CARS.md) ⚠️"] + + for title, category in (("## 🔀 Column Changes", "column"), ("## ❌ Removed", "removals"), + ("## ➕ Added", "additions"), ("## 📖 Detail Sentence Changes", "detail")): + if len(changes[category]): + markdown_builder.append(title) + if category not in ("detail",): + markdown_builder.append(COLUMNS) + markdown_builder.append(COLUMN_HEADER) + markdown_builder.extend(changes[category]) + + print("\n".join(markdown_builder)) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--path", required=True) + args = parser.parse_args() + print_car_docs_diff(args.path) diff --git a/selfdrive/debug/read_dtc_status.py b/selfdrive/debug/read_dtc_status.py new file mode 100644 index 0000000..9ad5563 --- /dev/null +++ b/selfdrive/debug/read_dtc_status.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +import sys +import argparse +from subprocess import check_output, CalledProcessError +from panda import Panda +from panda.python.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE +from panda.python.uds import get_dtc_num_as_str, get_dtc_status_names + +parser = argparse.ArgumentParser(description="read DTC status") +parser.add_argument("addr", type=lambda x: int(x,0)) +parser.add_argument("--bus", type=int, default=0) +parser.add_argument('--debug', action='store_true') +args = parser.parse_args() + +try: + check_output(["pidof", "boardd"]) + print("boardd is running, please kill openpilot before running this script! (aborted)") + sys.exit(1) +except CalledProcessError as e: + if e.returncode != 1: # 1 == no process found (boardd not running) + raise e + +panda = Panda() +panda.set_safety_mode(Panda.SAFETY_ELM327) +uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug) +print("extended diagnostic session ...") +uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) +print("read diagnostic codes ...") +data = uds_client.read_dtc_information(DTC_REPORT_TYPE.DTC_BY_STATUS_MASK, DTC_STATUS_MASK_TYPE.ALL) +print("status availability:", " ".join(get_dtc_status_names(data[0]))) +print("DTC status:") +for i in range(1, len(data), 4): + dtc_num = get_dtc_num_as_str(data[i:i+3]) + dtc_status = " ".join(get_dtc_status_names(data[i+3])) + print(dtc_num, dtc_status) diff --git a/selfdrive/debug/run_process_on_route.py b/selfdrive/debug/run_process_on_route.py new file mode 100644 index 0000000..90db14b --- /dev/null +++ b/selfdrive/debug/run_process_on_route.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 + +import argparse + +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, replay_process +from openpilot.tools.lib.helpers import save_log +from openpilot.tools.lib.logreader import LogReader + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run process on route and create new logs", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--fingerprint", help="The fingerprint to use") + parser.add_argument("route", help="The route name to use") + parser.add_argument("process", help="The process to run") + args = parser.parse_args() + + cfg = [c for c in CONFIGS if c.proc_name == args.process][0] + + lr = LogReader(args.route) + inputs = list(lr) + + outputs = replay_process(cfg, inputs, fingerprint=args.fingerprint) + + # Remove message generated by the process under test and merge in the new messages + produces = {o.which() for o in outputs} + inputs = [i for i in inputs if i.which() not in produces] + outputs = sorted(inputs + outputs, key=lambda x: x.logMonoTime) + + fn = f"{args.route.replace('/', '_')}_{args.process}.bz2" + print(f"Saved log to {fn}") + save_log(fn, outputs) diff --git a/selfdrive/debug/set_car_params.py b/selfdrive/debug/set_car_params.py new file mode 100644 index 0000000..6060dfb --- /dev/null +++ b/selfdrive/debug/set_car_params.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +import sys + +from cereal import car +from openpilot.common.params import Params +from openpilot.tools.lib.route import Route +from openpilot.tools.lib.logreader import LogReader + +if __name__ == "__main__": + CP = None + if len(sys.argv) > 1: + r = Route(sys.argv[1]) + cps = [m for m in LogReader(r.qlog_paths()[0]) if m.which() == 'carParams'] + CP = cps[0].carParams.as_builder() + else: + CP = car.CarParams.new_message() + CP.openpilotLongitudinalControl = True + CP.experimentalLongitudinalAvailable = False + + cp_bytes = CP.to_bytes() + for p in ("CarParams", "CarParamsCache", "CarParamsPersistent"): + Params().put(p, cp_bytes) diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py new file mode 100644 index 0000000..19144ea --- /dev/null +++ b/selfdrive/debug/show_matching_cars.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars +import cereal.messaging as messaging + + +# rav4 2019 and corolla tss2 +fingerprint = {896: 8, 898: 8, 900: 6, 976: 1, 1541: 8, 902: 6, 905: 8, 810: 2, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 1571: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 935: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 1592: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 1001: 8, 705: 8, 452: 8, 1788: 8, 464: 8, 824: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 934: 8, 998: 5, 1745: 8, 1000: 8, 764: 8, 1002: 8, 999: 7, 1789: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1786: 8, 1787: 8, 1020: 8, 426: 6, 1279: 8} # noqa: E501 + +candidate_cars = all_legacy_fingerprint_cars() + + +for addr, l in fingerprint.items(): + dat = messaging.new_message('can', 1) + + msg = dat.can[0] + msg.address = addr + msg.dat = " " * l + + candidate_cars = eliminate_incompatible_cars(msg, candidate_cars) + print(candidate_cars) diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py new file mode 100644 index 0000000..3c57335 --- /dev/null +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +# type: ignore + +from collections import defaultdict +import argparse +import os +import traceback +from tqdm import tqdm +from openpilot.tools.lib.logreader import LogReader, ReadMode +from openpilot.tools.lib.route import SegmentRange +from openpilot.selfdrive.car.car_helpers import interface_names +from openpilot.selfdrive.car.fingerprints import MIGRATION +from openpilot.selfdrive.car.fw_versions import VERSIONS, match_fw_to_car + + +NO_API = "NO_API" in os.environ +SUPPORTED_BRANDS = VERSIONS.keys() +SUPPORTED_CARS = [brand for brand in SUPPORTED_BRANDS for brand in interface_names[brand]] +UNKNOWN_BRAND = "unknown" + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Run FW fingerprint on Qlog of route or list of routes') + parser.add_argument('route', help='Route or file with list of routes') + parser.add_argument('--car', help='Force comparison fingerprint to known car') + args = parser.parse_args() + + if os.path.exists(args.route): + routes = list(open(args.route)) + else: + routes = [args.route] + + mismatches = defaultdict(list) + + not_fingerprinted = 0 + solved_by_fuzzy = 0 + + good_exact = 0 + wrong_fuzzy = 0 + good_fuzzy = 0 + + dongles = [] + for route in tqdm(routes): + sr = SegmentRange(route) + dongle_id = sr.dongle_id + + if dongle_id in dongles: + continue + + if sr.slice == '' and sr.selector is None: + route += '/0' + + lr = LogReader(route, default_mode=ReadMode.QLOG) + + try: + dongles.append(dongle_id) + + CP = None + for msg in lr: + if msg.which() == "pandaStates": + if msg.pandaStates[0].pandaType in ('unknown', 'whitePanda', 'greyPanda', 'pedal'): + print("wrong panda type") + break + + elif msg.which() == "carParams": + CP = msg.carParams + car_fw = [fw for fw in CP.carFw if not fw.logging] + if len(car_fw) == 0: + print("no fw") + break + + live_fingerprint = CP.carFingerprint + live_fingerprint = MIGRATION.get(live_fingerprint, live_fingerprint) + + if args.car is not None: + live_fingerprint = args.car + + if live_fingerprint not in SUPPORTED_CARS: + print("not in supported cars") + break + + _, exact_matches = match_fw_to_car(car_fw, allow_exact=True, allow_fuzzy=False) + _, fuzzy_matches = match_fw_to_car(car_fw, allow_exact=False, allow_fuzzy=True) + + if (len(exact_matches) == 1) and (list(exact_matches)[0] == live_fingerprint): + good_exact += 1 + print(f"Correct! Live: {live_fingerprint} - Fuzzy: {fuzzy_matches}") + + # Check if fuzzy match was correct + if len(fuzzy_matches) == 1: + if list(fuzzy_matches)[0] != live_fingerprint: + wrong_fuzzy += 1 + print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) + else: + good_fuzzy += 1 + break + + print("Old style:", live_fingerprint, "Vin", CP.carVin) + print("New style (exact):", exact_matches) + print("New style (fuzzy):", fuzzy_matches) + + padding = max([len(fw.brand or UNKNOWN_BRAND) for fw in car_fw]) + for version in sorted(car_fw, key=lambda fw: fw.brand): + subaddr = None if version.subAddress == 0 else hex(version.subAddress) + print(f" Brand: {version.brand or UNKNOWN_BRAND:{padding}}, bus: {version.bus} - " + + f"(Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}],") + + print("Mismatches") + found = False + for brand in SUPPORTED_BRANDS: + car_fws = VERSIONS[brand] + if live_fingerprint in car_fws: + found = True + expected = car_fws[live_fingerprint] + for (_, expected_addr, expected_sub_addr), v in expected.items(): + for version in car_fw: + if version.brand != brand and len(version.brand): + continue + sub_addr = None if version.subAddress == 0 else version.subAddress + addr = version.address + + if (addr, sub_addr) == (expected_addr, expected_sub_addr): + if version.fwVersion not in v: + print(f"({hex(addr)}, {'None' if sub_addr is None else hex(sub_addr)}) - {version.fwVersion}") + + # Add to global list of mismatches + mismatch = (addr, sub_addr, version.fwVersion) + if mismatch not in mismatches[live_fingerprint]: + mismatches[live_fingerprint].append(mismatch) + + # No FW versions for this car yet, add them all to mismatch list + if not found: + for version in car_fw: + sub_addr = None if version.subAddress == 0 else version.subAddress + addr = version.address + mismatch = (addr, sub_addr, version.fwVersion) + if mismatch not in mismatches[live_fingerprint]: + mismatches[live_fingerprint].append(mismatch) + + print() + not_fingerprinted += 1 + + if len(fuzzy_matches) == 1: + if list(fuzzy_matches)[0] == live_fingerprint: + solved_by_fuzzy += 1 + else: + wrong_fuzzy += 1 + print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) + + break + + if CP is None: + print("no CarParams in logs") + except Exception: + traceback.print_exc() + except KeyboardInterrupt: + break + + print() + # Print FW versions that need to be added separated out by car and address + for car, m in sorted(mismatches.items()): + print(car) + addrs = defaultdict(list) + for (addr, sub_addr, version) in m: + addrs[(addr, sub_addr)].append(version) + + for (addr, sub_addr), versions in addrs.items(): + print(f" ({hex(addr)}, {'None' if sub_addr is None else hex(sub_addr)}): [") + for v in versions: + print(f" {v},") + print(" ]") + print() + + print() + print(f"Number of dongle ids checked: {len(dongles)}") + print(f"Fingerprinted: {good_exact}") + print(f"Not fingerprinted: {not_fingerprinted}") + print(f" of which had a fuzzy match: {solved_by_fuzzy}") + + print() + print(f"Correct fuzzy matches: {good_fuzzy}") + print(f"Wrong fuzzy matches: {wrong_fuzzy}") + print() + diff --git a/selfdrive/debug/toyota_eps_factor.py b/selfdrive/debug/toyota_eps_factor.py new file mode 100644 index 0000000..dc83c8f --- /dev/null +++ b/selfdrive/debug/toyota_eps_factor.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import sys +import numpy as np +import matplotlib.pyplot as plt +from sklearn import linear_model +from openpilot.selfdrive.car.toyota.values import STEER_THRESHOLD + +from openpilot.tools.lib.logreader import LogReader + +MIN_SAMPLES = 30 * 100 + + +def to_signed(n, bits): + if n >= (1 << max((bits - 1), 0)): + n = n - (1 << max(bits, 0)) + return n + + +def get_eps_factor(lr, plot=False): + engaged = False + steering_pressed = False + torque_cmd, eps_torque = None, None + cmds, eps = [], [] + + for msg in lr: + if msg.which() != 'can': + continue + + for m in msg.can: + if m.address == 0x2e4 and m.src == 128: + engaged = bool(m.dat[0] & 1) + torque_cmd = to_signed((m.dat[1] << 8) | m.dat[2], 16) + elif m.address == 0x260 and m.src == 0: + eps_torque = to_signed((m.dat[5] << 8) | m.dat[6], 16) + steering_pressed = abs(to_signed((m.dat[1] << 8) | m.dat[2], 16)) > STEER_THRESHOLD + + if engaged and torque_cmd is not None and eps_torque is not None and not steering_pressed: + cmds.append(torque_cmd) + eps.append(eps_torque) + else: + if len(cmds) > MIN_SAMPLES: + break + cmds, eps = [], [] + + if len(cmds) < MIN_SAMPLES: + raise Exception("too few samples found in route") + + lm = linear_model.LinearRegression(fit_intercept=False) + lm.fit(np.array(cmds).reshape(-1, 1), eps) + scale_factor = 1. / lm.coef_[0] + + if plot: + plt.plot(np.array(eps) * scale_factor) + plt.plot(cmds) + plt.show() + return scale_factor + + +if __name__ == "__main__": + lr = LogReader(sys.argv[1]) + n = get_eps_factor(lr, plot="--plot" in sys.argv) + print("EPS torque factor: ", n) diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py new file mode 100755 index 0000000..f4440a9 --- /dev/null +++ b/selfdrive/debug/uiview.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import time + +from cereal import car, log, messaging +from openpilot.common.params import Params +from openpilot.selfdrive.manager.process_config import managed_processes + +if __name__ == "__main__": + CP = car.CarParams(notCar=True) + Params().put("CarParams", CP.to_bytes()) + + procs = ['camerad', 'ui', 'modeld', 'calibrationd'] + for p in procs: + managed_processes[p].start() + + pm = messaging.PubMaster(['controlsState', 'deviceState', 'pandaStates', 'carParams']) + + msgs = {s: messaging.new_message(s) for s in ['controlsState', 'deviceState', 'carParams']} + msgs['deviceState'].deviceState.started = True + msgs['carParams'].carParams.openpilotLongitudinalControl = True + + msgs['pandaStates'] = messaging.new_message('pandaStates', 1) + msgs['pandaStates'].pandaStates[0].ignitionLine = True + msgs['pandaStates'].pandaStates[0].pandaType = log.PandaState.PandaType.uno + + try: + while True: + time.sleep(1 / 100) # continually send, rate doesn't matter + for s in msgs: + pm.send(s, msgs[s]) + except KeyboardInterrupt: + for p in procs: + managed_processes[p].stop() diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py new file mode 100755 index 0000000..75409e3 --- /dev/null +++ b/selfdrive/debug/vw_mqb_config.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 + +import argparse +import struct +from enum import IntEnum +from panda import Panda +from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\ + DATA_IDENTIFIER_TYPE, ACCESS_TYPE + +# TODO: extend UDS library to allow custom/vendor-defined data identifiers without ignoring type checks +class VOLKSWAGEN_DATA_IDENTIFIER_TYPE(IntEnum): + CODING = 0x0600 + +# TODO: extend UDS library security_access() to take an access level offset per ISO 14229-1:2020 10.4 and remove this +class ACCESS_TYPE_LEVEL_1(IntEnum): + REQUEST_SEED = ACCESS_TYPE.REQUEST_SEED + 2 + SEND_KEY = ACCESS_TYPE.SEND_KEY + 2 + +MQB_EPS_CAN_ADDR = 0x712 +RX_OFFSET = 0x6a + +if __name__ == "__main__": + desc_text = "Shows Volkswagen EPS software and coding info, and enables or disables Heading Control Assist " + \ + "(Lane Assist). Useful for enabling HCA on cars without factory Lane Assist that want to use " + \ + "openpilot integrated at the CAN gateway (J533)." + epilog_text = "This tool is meant to run directly on a vehicle-installed comma three, with the " + \ + "openpilot/tmux processes stopped. It should also work on a separate PC with a USB-attached comma " + \ + "panda. Vehicle ignition must be on. Recommend engine not be running when making changes. Must " + \ + "turn ignition off and on again for any changes to take effect." + parser = argparse.ArgumentParser(description=desc_text, epilog=epilog_text) + parser.add_argument("--debug", action="store_true", help="enable ISO-TP/UDS stack debugging output") + parser.add_argument("action", choices={"show", "enable", "disable"}, help="show or modify current EPS HCA config") + args = parser.parse_args() + + panda = Panda() + panda.set_safety_mode(Panda.SAFETY_ELM327) + bus = 1 if panda.has_obd() else 0 + uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, bus, timeout=0.2, debug=args.debug) + + try: + uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) + except MessageTimeoutError: + print("Timeout opening session with EPS") + quit() + + odx_file, current_coding = None, None + try: + hw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER).decode("utf-8") + sw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER).decode("utf-8") + sw_ver = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER).decode("utf-8") + component = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.SYSTEM_NAME_OR_ENGINE_TYPE).decode("utf-8") + odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8").rstrip('\x00') + current_coding = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING) # type: ignore + coding_text = current_coding.hex() + + print("\nEPS diagnostic data\n") + print(f" Part No HW: {hw_pn}") + print(f" Part No SW: {sw_pn}") + print(f" SW Version: {sw_ver}") + print(f" Component: {component}") + print(f" Coding: {coding_text}") + print(f" ASAM Dataset: {odx_file}") + except NegativeResponseError: + print("Error fetching data from EPS") + quit() + except MessageTimeoutError: + print("Timeout fetching data from EPS") + quit() + + coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 + coding_length = len(current_coding) + + # EPS_MQB_ZFLS + if odx_file in ("EV_SteerAssisMQB", "EV_SteerAssisMNB"): + coding_variant = "ZFLS" + coding_byte = 0 + coding_bit = 4 + + # MQB_PP_APA, MQB_VWBS_GEN2 + elif odx_file in ("EV_SteerAssisVWBSMQBA", "EV_SteerAssisVWBSMQBGen2"): + coding_variant = "APA" + coding_byte = 3 + coding_bit = 0 + + else: + print("Configuration changes not yet supported on this EPS!") + quit() + + current_coding_array = struct.unpack(f"!{coding_length}B", current_coding) + hca_enabled = (current_coding_array[coding_byte] & (1 << coding_bit) != 0) + hca_text = ("DISABLED", "ENABLED")[hca_enabled] + print(f" Lane Assist: {hca_text}") + + try: + params = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION).decode("utf-8") + param_version_system_params = params[1:3] + param_vehicle_type = params[3:5] + param_index_char_curve = params[5:7] + param_version_char_values = params[7:9] + param_version_memory_map = params[9:11] + print("\nEPS parameterization (per-vehicle calibration) data\n") + print(f" Version of system parameters: {param_version_system_params}") + print(f" Vehicle type: {param_vehicle_type}") + print(f" Index of characteristic curve: {param_index_char_curve}") + print(f" Version of characteristic values: {param_version_char_values}") + print(f" Version of memory map: {param_version_memory_map}") + except (NegativeResponseError, MessageTimeoutError): + print("Error fetching parameterization data from EPS!") + quit() + + if args.action in ["enable", "disable"]: + print("\nAttempting configuration update") + + assert(coding_variant in ("ZFLS", "APA")) + # ZFLS EPS config coding length can be anywhere from 1 to 4 bytes, but the + # bit we care about is always in the same place in the first byte + if args.action == "enable": + new_byte = current_coding_array[coding_byte] | (1 << coding_bit) + else: + new_byte = current_coding_array[coding_byte] & ~(1 << coding_bit) + new_coding = current_coding[0:coding_byte] + new_byte.to_bytes(1, "little") + current_coding[coding_byte+1:] + + try: + seed = uds_client.security_access(ACCESS_TYPE_LEVEL_1.REQUEST_SEED) # type: ignore + key = struct.unpack("!I", seed)[0] + 28183 # yeah, it's like that + uds_client.security_access(ACCESS_TYPE_LEVEL_1.SEND_KEY, struct.pack("!I", key)) # type: ignore + except (NegativeResponseError, MessageTimeoutError): + print("Security access failed!") + print("Open the hood and retry (disables the \"diagnostic firewall\" on newer vehicles)") + quit() + + try: + # Programming date and tester number must be written before making + # a change, or write to CODING will fail with request sequence error + # Encoding on tester is unclear, it contains the workshop code in the + # last two bytes, but not the VZ/importer or tester serial number + # Can't seem to read it back, but we can read the calibration tester, + # so fib a little and say that same tester did the programming + # TODO: encode the actual current date + prog_date = b'\x22\x02\x08' + uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.PROGRAMMING_DATE, prog_date) + tester_num = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.CALIBRATION_REPAIR_SHOP_CODE_OR_CALIBRATION_EQUIPMENT_SERIAL_NUMBER) + uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.REPAIR_SHOP_CODE_OR_TESTER_SERIAL_NUMBER, tester_num) + uds_client.write_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING, new_coding) # type: ignore + except (NegativeResponseError, MessageTimeoutError): + print("Writing new configuration failed!") + print("Make sure the comma processes are stopped: tmux kill-session -t comma") + quit() + + try: + # Read back result just to make 100% sure everything worked + current_coding_text = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING).hex() # type: ignore + print(f" New coding: {current_coding_text}") + except (NegativeResponseError, MessageTimeoutError): + print("Reading back updated coding failed!") + quit() + print("EPS configuration successfully updated") diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index f2758cf..067e1b5 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -2,7 +2,6 @@ import os import subprocess from pathlib import Path -from typing import List # NOTE: Do NOT import anything here that needs be built (e.g. params) from openpilot.common.basedir import BASEDIR @@ -29,7 +28,7 @@ def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: # building with all cores can result in using too # much memory, so retry with less parallelism - compile_output: List[bytes] = [] + compile_output: list[bytes] = [] for n in (nproc, nproc/2, 1): compile_output.clear() scons: subprocess.Popen = subprocess.Popen(["scons", f"-j{int(n)}", "--cache-populate", *extra_args], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index d750a7d..8745fef 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -5,16 +5,15 @@ import signal import subprocess import sys import threading +import time import traceback -from pathlib import Path -from typing import List, Tuple, Union from cereal import log import cereal.messaging as messaging import openpilot.selfdrive.sentry as sentry -from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params, ParamKeyType from openpilot.common.text_window import TextWindow +from openpilot.common.time import system_time_valid from openpilot.system.hardware import HARDWARE, PC from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog from openpilot.selfdrive.manager.process import ensure_running @@ -25,63 +24,61 @@ from openpilot.system.version import is_dirty, get_commit, get_version, get_orig get_normalized_origin, terms_version, training_version, \ is_tested_branch, is_release_branch, get_commit_date -from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import DEFAULT_MODEL +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, delete_deprecated_models -def manager_init() -> None: +def frogpilot_boot_functions(frogpilot_functions): + try: + delete_deprecated_models() + + while not system_time_valid(): + print("Waiting for system time to become valid...") + time.sleep(1) + + try: + frogpilot_functions.backup_frogpilot() + except subprocess.CalledProcessError as e: + print(f"Failed to backup FrogPilot. Error: {e}") + return + + try: + frogpilot_functions.backup_toggles() + except subprocess.CalledProcessError as e: + print(f"Failed to backup toggles. Error: {e}") + return + + except Exception as e: + print(f"An unexpected error occurred: {e}") + +def manager_init(frogpilot_functions) -> None: + frogpilot_boot = threading.Thread(target=frogpilot_boot_functions, args=(frogpilot_functions,)) + frogpilot_boot.start() + save_bootlog() params = Params() - params_storage = Params("/persist/comma/params") + params_storage = Params("/persist/params") params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) if is_release_branch(): params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) - ############### Remove this after the April 26th update ############### - - previous_speed_limit = params.get_float("PreviousSpeedLimit") - if previous_speed_limit >= 50: - params.put_float("PreviousSpeedLimit", previous_speed_limit / 100) - - for priority_key in ["SLCPriority", "SLCPriority1", "SLCPriority2", "SLCPriority3"]: - priority_value = params.get(priority_key) - if isinstance(priority_value, int): - params.remove(priority_key) - - attributes = ["AggressiveFollow", "StandardFollow", "RelaxedFollow", "AggressiveJerk", "StandardJerk", "RelaxedJerk"] - values = {attr: params.get_float(attr) for attr in attributes} - if any(value > 5 for value in values.values()): - for attr, value in values.items(): - params.put_float(attr, value / 10) - - if params.get_bool("SilentMode"): - attributes = ["DisengageVolume", "EngageVolume", "PromptVolume", "PromptDistractedVolume", "RefuseVolume", "WarningSoftVolume", "WarningImmediateVolume"] - for attr in attributes: - params.put_float(attr, 0) - params.put_bool("SilentMode", False) - - ####################################################################### - - # Check if the currently selected model still exists - current_model = params.get("Model", encoding='utf-8') - - if current_model != DEFAULT_MODEL: - models_folder = os.path.join(BASEDIR, 'selfdrive/modeld/models/models') - model_exists = current_model in [os.path.splitext(file)[0] for file in os.listdir(models_folder)] - - if not model_exists: - params.remove("Model") - - default_params: List[Tuple[str, Union[str, bytes]]] = [ + default_params: list[tuple[str, str | bytes]] = [ + ("CarParamsPersistent", ""), ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), + ("ExperimentalLongitudinalEnabled", "1"), ("GsmMetered", "1"), ("HasAcceptedTerms", "0"), + ("IsLdwEnabled", "0"), + ("IsMetric", "0"), ("LanguageSetting", "main_en"), + ("NavSettingLeftSide", "0"), + ("NavSettingTime24h", "0"), ("OpenpilotEnabledToggle", "1"), - ("UpdaterAvailableBranches", ""), + ("RecordFront", "0"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), # Default FrogPilot parameters @@ -89,15 +86,17 @@ def manager_init() -> None: ("AccelerationProfile", "2"), ("AdjacentPath", "0"), ("AdjacentPathMetrics", "0"), - ("AdjustablePersonalities", "1"), ("AggressiveAcceleration", "1"), ("AggressiveFollow", "1.25"), ("AggressiveJerk", "0.5"), ("AlertVolumeControl", "0"), ("AlwaysOnLateral", "1"), ("AlwaysOnLateralMain", "0"), + ("AMapKey1", ""), + ("AMapKey2", ""), + ("AutomaticUpdates", "0"), ("BlindSpotPath", "1"), - ("CameraView", "1"), + ("CameraView", "2"), ("CarMake", ""), ("CarModel", ""), ("CECurves", "1"), @@ -106,18 +105,21 @@ def manager_init() -> None: ("CENavigationLead", "1"), ("CENavigationTurns", "1"), ("CESignal", "1"), - ("CESlowerLead", "0"), + ("CESlowerLead", "1"), ("CESpeed", "0"), ("CESpeedLead", "0"), ("CEStopLights", "1"), ("CEStopLightsLead", "0"), - ("Compass", "0"), + ("Compass", "1"), ("ConditionalExperimental", "1"), ("CrosstrekTorque", "1"), ("CurveSensitivity", "100"), ("CustomAlerts", "1"), ("CustomColors", "1"), + ("CustomCruise", "1"), + ("CustomCruiseLong", "5"), ("CustomIcons", "1"), + ("CustomPaths", "1"), ("CustomPersonalities", "1"), ("CustomSignals", "1"), ("CustomSounds", "1"), @@ -125,6 +127,8 @@ def manager_init() -> None: ("CustomUI", "1"), ("CydiaTune", "0"), ("DecelerationProfile", "1"), + ("DeveloperUI", "0"), + ("DeviceManagement", "1"), ("DeviceShutdown", "9"), ("DisableMTSCSmoothing", "0"), ("DisableOnroadUploads", "0"), @@ -133,26 +137,22 @@ def manager_init() -> None: ("DisengageVolume", "100"), ("DragonPilotTune", "0"), ("DriverCamera", "0"), - ("DriveStats", "1"), ("DynamicPathWidth", "0"), ("EngageVolume", "100"), ("EVTable", "1"), ("ExperimentalModeActivation", "1"), - ("ExperimentalModeViaDistance", "0"), - ("ExperimentalModeViaLKAS", "0"), - ("ExperimentalModeViaScreen", "1"), + ("ExperimentalModeViaDistance", "1"), + ("ExperimentalModeViaLKAS", "1"), + ("ExperimentalModeViaTap", "0"), ("Fahrenheit", "0"), - ("FireTheBabysitter", "0"), - ("ForceAutoTune", "0"), + ("ForceAutoTune", "1"), ("ForceFingerprint", "0"), ("ForceMPHDashboard", "0"), ("FPSCounter", "0"), - ("FrogPilotDrives", "0"), - ("FrogPilotKilometers", "0"), - ("FrogPilotMinutes", "0"), ("FrogsGoMooTune", "1"), ("FullMap", "0"), ("GasRegenCmd", "0"), + ("GMapKey", ""), ("GoatScream", "1"), ("GreenLightAlert", "0"), ("HideAlerts", "0"), @@ -166,46 +166,56 @@ def manager_init() -> None: ("HideUIElements", "0"), ("HigherBitrate", "0"), ("HolidayThemes", "1"), + ("IncreaseThermalLimits", "0"), ("LaneChangeTime", "0"), - ("LaneDetection", "1"), ("LaneDetectionWidth", "60"), ("LaneLinesWidth", "4"), ("LateralTune", "1"), ("LeadDepartingAlert", "0"), - ("LeadDetectionThreshold", "25"), + ("LeadDetectionThreshold", "35"), ("LeadInfo", "0"), - ("LockDoors", "0"), + ("LockDoors", "1"), ("LongitudinalTune", "1"), ("LongPitch", "1"), ("LoudBlindspotAlert", "0"), - ("LowerVolt", "1"), + ("LowVoltageShutdown", "11.8"), + ("kiV1", "0.60"), + ("kiV2", "0.45"), + ("kiV3", "0.30"), + ("kiV4", "0.15"), + ("kpV1", "1.50"), + ("kpV2", "1.00"), + ("kpV3", "0.75"), + ("kpV4", "0.50"), ("MapsSelected", ""), + ("MapboxPublicKey", ""), + ("MapboxSecretKey", ""), ("MapStyle", "0"), ("MTSCAggressiveness", "100"), ("MTSCCurvatureCheck", "0"), - ("MTSCLimit", "0"), ("Model", DEFAULT_MODEL), + ("ModelName", DEFAULT_MODEL_NAME), + ("ModelSelector", "1"), ("ModelUI", "1"), ("MTSCEnabled", "1"), - ("MuteOverheated", "0"), - ("NavChill", "0"), ("NNFF", "1"), + ("NNFFLite", "1"), ("NoLogging", "0"), ("NoUploads", "0"), ("NudgelessLaneChange", "1"), ("NumericalTemp", "0"), - ("OfflineMode", "0"), + ("OfflineMode", "1"), ("Offset1", "5"), ("Offset2", "5"), ("Offset3", "5"), ("Offset4", "10"), ("OneLaneChange", "1"), + ("OnroadDistanceButton", "0"), ("PathEdgeWidth", "20"), ("PathWidth", "61"), + ("PauseAOLOnBrake", "0"), ("PauseLateralOnSignal", "0"), ("PedalsOnUI", "1"), - ("PersonalitiesViaScreen", "1"), - ("PersonalitiesViaWheel", "1"), ("PreferredSchedule", "0"), ("PromptVolume", "100"), ("PromptDistractedVolume", "100"), @@ -232,21 +242,27 @@ def manager_init() -> None: ("ShowCPU", "0"), ("ShowGPU", "0"), ("ShowIP", "0"), + ("ShowJerk", "1"), ("ShowMemoryUsage", "0"), - ("ShowSLCOffset", "0"), + ("ShowSLCOffset", "1"), ("ShowSLCOffsetUI", "1"), ("ShowStorageLeft", "0"), ("ShowStorageUsed", "0"), + ("ShowTuning", "1"), ("Sidebar", "0"), ("SLCConfirmation", "1"), ("SLCConfirmationLower", "1"), ("SLCConfirmationHigher", "1"), ("SLCFallback", "2"), + ("SLCLookaheadHigher", "5"), + ("SLCLookaheadLower", "5"), ("SLCOverride", "1"), ("SLCPriority1", "Dashboard"), ("SLCPriority2", "Offline Maps"), ("SLCPriority3", "Navigation"), ("SmoothBraking", "1"), + ("SmoothBrakingFarLead", "0"), + ("SmoothBrakingJerk", "0"), ("SNGHack", "1"), ("SpeedLimitChangedAlert", "1"), ("SpeedLimitController", "1"), @@ -256,13 +272,17 @@ def manager_init() -> None: ("SteerRatio", "0"), ("StockTune", "0"), ("StoppingDistance", "0"), + ("TacoTune", "1"), + ("ToyotaDoors", "0"), + ("TrafficFollow", "0.5"), + ("TrafficJerk", "1"), ("TrafficMode", "0"), + ("Tuning", "1"), ("TurnAggressiveness", "100"), ("TurnDesires", "0"), ("UnlimitedLength", "1"), - ("UpdateSchedule", "0"), - ("UseLateralJerk", "0"), - ("UseSI", "0"), + ("UnlockDoors", "1"), + ("UseSI", "1"), ("UseVienna", "0"), ("VisionTurnControl", "1"), ("WarningSoftVolume", "100"), @@ -347,23 +367,15 @@ def manager_cleanup() -> None: cloudlog.info("everything is dead") -def update_frogpilot_params(params, params_memory): - keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "NoUploads", "RoadNameUI", "SpeedLimitController"] - for key in keys: - params_memory.put_bool(key, params.get_bool(key)) - -def manager_thread() -> None: +def manager_thread(frogpilot_functions) -> None: cloudlog.bind(daemon="manager") cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) params = Params() params_memory = Params("/dev/shm/params") - params_storage = Params("/persist/comma/params") - update_frogpilot_params(params, params_memory) - - ignore: List[str] = [] + ignore: list[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: @@ -374,20 +386,23 @@ def manager_thread() -> None: pm = messaging.PubMaster(['managerState']) write_onroad_params(False, params) - ensure_running(managed_processes.values(), False, params=params, params_memory=params_memory, CP=sm['carParams'], not_run=ignore) + ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) started_prev = False while True: sm.update(1000) + openpilot_crashed = os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) + if openpilot_crashed: + frogpilot_functions.delete_logs() + started = sm['deviceState'].started if started and not started_prev: params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) - # Clear the error log on offroad transition to prevent old errors from hanging around - if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')): + if openpilot_crashed: os.remove(os.path.join(sentry.CRASHES_DIR, 'error.txt')) elif not started and started_prev: @@ -400,9 +415,9 @@ def manager_thread() -> None: started_prev = started - ensure_running(managed_processes.values(), started, params=params, params_memory=params_memory, CP=sm['carParams'], not_run=ignore) + ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) - running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) + running = ' '.join("{}{}\u001b[0m".format("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc) print(running) cloudlog.debug(running) @@ -414,7 +429,7 @@ def manager_thread() -> None: # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False - for param in ("DoUninstall", "DoShutdown", "DoReboot"): + for param in ("DoUninstall", "DoShutdown", "DoReboot", "DoSoftReboot"): if params.get_bool(param): shutdown = True params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}") @@ -423,55 +438,17 @@ def manager_thread() -> None: if shutdown: break - if params_memory.get_bool("FrogPilotTogglesUpdated"): - update_frogpilot_params(params, params_memory) - -def backup_openpilot(): - # Configure the auto backup generator - backup_dir_path = '/data/backups' - Path(backup_dir_path).mkdir(parents=True, exist_ok=True) - - # Sort backups by creation time and only keep the 5 newest auto generated ones - auto_backups = sorted([f for f in os.listdir(backup_dir_path) if f.endswith("_auto")], - key=lambda x: os.path.getmtime(os.path.join(backup_dir_path, x))) - for old_backup in auto_backups: - subprocess.run(['sudo', 'rm', '-rf', os.path.join(backup_dir_path, old_backup)], check=True) - print(f"Deleted oldest backup to maintain limit: {old_backup}") - - # Generate the backup folder name from the current git commit and branch name - branch = get_short_branch() - commit = get_commit_date()[12:-16] - backup_folder_name = f"{branch}_{commit}_auto" - - # Check if the backup folder already exists - backup_path = os.path.join(backup_dir_path, backup_folder_name) - - if os.path.exists(backup_path): - print(f"Backup folder {backup_folder_name} already exists. Skipping backup.") - return - - # Create the backup directory and copy openpilot to it - Path(backup_path).mkdir(parents=True, exist_ok=True) - subprocess.run(['sudo', 'cp', '-a', '/data/openpilot/.', backup_path + '/'], check=True) - print(f"Successfully backed up openpilot to {backup_folder_name}.") def main() -> None: - # Create the long term param storage folder - try: - # Attempt to remount /persist as read-write - subprocess.run(['sudo', 'mount', '-o', 'remount,rw', '/persist'], check=True) - print("Successfully remounted /persist as read-write.") - except subprocess.CalledProcessError as e: - print(f"Failed to remount /persist. Error: {e}") + frogpilot_functions = FrogPilotFunctions() - # Backup the current version of openpilot try: - backup_thread = threading.Thread(target=backup_openpilot) - backup_thread.start() + frogpilot_functions.setup_frogpilot() except subprocess.CalledProcessError as e: - print(f"Failed to backup openpilot. Error: {e}") + print(f"Failed to setup FrogPilot. Error: {e}") + return - manager_init() + manager_init(frogpilot_functions) if os.getenv("PREPAREONLY") is not None: return @@ -479,7 +456,7 @@ def main() -> None: signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) try: - manager_thread() + manager_thread(frogpilot_functions) except Exception: traceback.print_exc() sentry.capture_exception() @@ -489,10 +466,13 @@ def main() -> None: params = Params() if params.get_bool("DoUninstall"): cloudlog.warning("uninstalling") - HARDWARE.uninstall() + frogpilot_functions.uninstall_frogpilot() elif params.get_bool("DoReboot"): cloudlog.warning("reboot") HARDWARE.reboot() + elif params.get_bool("DoSoftReboot"): + cloudlog.warning("softreboot") + HARDWARE.soft_reboot() elif params.get_bool("DoShutdown"): cloudlog.warning("shutdown") HARDWARE.shutdown() diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 7bd7c91..607d3d0 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -4,7 +4,7 @@ import signal import struct import time import subprocess -from typing import Optional, Callable, List, ValuesView +from collections.abc import Callable, ValuesView from abc import ABC, abstractmethod from multiprocessing import Process @@ -47,7 +47,7 @@ def launcher(proc: str, name: str) -> None: raise -def nativelauncher(pargs: List[str], cwd: str, name: str) -> None: +def nativelauncher(pargs: list[str], cwd: str, name: str) -> None: os.environ['MANAGER_DAEMON'] = name # exec the process @@ -67,12 +67,12 @@ class ManagerProcess(ABC): daemon = False sigkill = False should_run: Callable[[bool, Params, car.CarParams], bool] - proc: Optional[Process] = None + proc: Process | None = None enabled = True name = "" last_watchdog_time = 0 - watchdog_max_dt: Optional[int] = None + watchdog_max_dt: int | None = None watchdog_seen = False shutting_down = False @@ -109,7 +109,7 @@ class ManagerProcess(ABC): else: self.watchdog_seen = True - def stop(self, retry: bool = True, block: bool = True, sig: Optional[signal.Signals] = None) -> Optional[int]: + def stop(self, retry: bool = True, block: bool = True, sig: signal.Signals = None) -> int | None: if self.proc is None: return None @@ -239,7 +239,7 @@ class DaemonProcess(ManagerProcess): self.params = None @staticmethod - def should_run(started, params, params_memory, CP): + def should_run(started, params, CP): return True def prepare(self) -> None: @@ -274,14 +274,14 @@ class DaemonProcess(ManagerProcess): pass -def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, params_memory=None, CP: car.CarParams=None, - not_run: Optional[List[str]]=None) -> List[ManagerProcess]: +def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, + not_run: list[str] | None=None) -> list[ManagerProcess]: if not_run is None: not_run = [] running = [] for p in procs: - if p.enabled and p.name not in not_run and p.should_run(started, params, params_memory, CP): + if p.enabled and p.name not in not_run and p.should_run(started, params, CP): running.append(p) else: p.stop(block=False) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 1905210..7fcbddb 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -5,51 +5,53 @@ from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS + +RADARLESS = Params().get("Model", encoding='utf-8') in RADARLESS_MODELS WEBCAM = os.getenv("USE_WEBCAM") is not None -def driverview(started: bool, params: Params, params_memory: Params, CP: car.CarParams) -> bool: +def driverview(started: bool, params: Params, CP: car.CarParams) -> bool: return started or params.get_bool("IsDriverViewEnabled") -def notcar(started: bool, params: Params, params_memory: Params, CP: car.CarParams) -> bool: +def notcar(started: bool, params: Params, CP: car.CarParams) -> bool: return started and CP.notCar -def iscar(started: bool, params: Params, params_memory: Params, CP: car.CarParams) -> bool: +def iscar(started: bool, params: Params, CP: car.CarParams) -> bool: return started and not CP.notCar -def logging(started, params, params_memory, CP: car.CarParams) -> bool: +def logging(started, params, CP: car.CarParams) -> bool: run = (not CP.notCar) or not params.get_bool("DisableLogging") return started and run def ublox_available() -> bool: return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') -def ublox(started, params, params_memory, CP: car.CarParams) -> bool: +def ublox(started, params, CP: car.CarParams) -> bool: use_ublox = ublox_available() if use_ublox != params.get_bool("UbloxAvailable"): params.put_bool("UbloxAvailable", use_ublox) return started and use_ublox -def qcomgps(started, params, params_memory, CP: car.CarParams) -> bool: +def qcomgps(started, params, CP: car.CarParams) -> bool: return started and not ublox_available() -def always_run(started, params, params_memory, CP: car.CarParams) -> bool: +def always_run(started, params, CP: car.CarParams) -> bool: return True -def only_onroad(started: bool, params, params_memory, CP: car.CarParams) -> bool: +def only_onroad(started: bool, params, CP: car.CarParams) -> bool: return started -def only_offroad(started, params, params_memory, CP: car.CarParams) -> bool: +def only_offroad(started, params, CP: car.CarParams) -> bool: return not started # FrogPilot functions -def allow_uploads(started, params, params_memory, CP: car.CarParams) -> bool: - allow_uploads = not (params_memory.get_bool("FireTheBabysitter") and params_memory.get_bool("NoUploads")) - at_home = not started or not params_memory.get_bool("DisableOnroadUploads") - return allow_uploads and at_home +def allow_logging(started, params, CP: car.CarParams) -> bool: + allow_logging = not (params.get_bool("DeviceManagement") and params.get_bool("NoLogging")) + return allow_logging and logging(started, params, CP) -def allow_logging(started, params, params_memory, CP: car.CarParams) -> bool: - allow_logging = not (params_memory.get_bool("FireTheBabysitter") and params_memory.get_bool("NoLogging")) - return allow_logging and logging(started, params, params_memory, CP) +def allow_uploads(started, params, CP: car.CarParams) -> bool: + allow_uploads = not (params.get_bool("DeviceManagement") and params.get_bool("NoUploads")) + return allow_uploads procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), @@ -79,16 +81,18 @@ procs = [ PythonProcess("deleter", "system.loggerd.deleter", always_run), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)), PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), + #PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI), PythonProcess("navd", "selfdrive.navd.navd", only_onroad), PythonProcess("pandad", "selfdrive.boardd.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), - PythonProcess("pigeond", "system.sensord.pigeond", ublox, enabled=TICI), + PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad), - PythonProcess("radard", "selfdrive.controls.radard", only_onroad), + PythonProcess("radard", "selfdrive.controls.radard", only_onroad, enabled=not RADARLESS), + PythonProcess("radardless", "selfdrive.controls.radardless", only_onroad, enabled=RADARLESS), PythonProcess("thermald", "selfdrive.thermald.thermald", always_run), - PythonProcess("tombstoned", "selfdrive.tombstoned", allow_logging, enabled=not PC), - PythonProcess("updated", "selfdrive.updated", always_run, enabled=not PC), + PythonProcess("tombstoned", "selfdrive.tombstoned", always_run, enabled=not PC), + PythonProcess("updated", "selfdrive.updated.updated", always_run, enabled=not PC), PythonProcess("uploader", "system.loggerd.uploader", allow_uploads), PythonProcess("statsd", "selfdrive.statsd", allow_logging), @@ -99,8 +103,8 @@ procs = [ # FrogPilot processes PythonProcess("fleet_manager", "selfdrive.frogpilot.fleetmanager.fleet_manager", always_run), - PythonProcess("frogpilot_process", "selfdrive.frogpilot.functions.frogpilot_process", always_run), - PythonProcess("mapd", "selfdrive.frogpilot.functions.mapd", always_run), + PythonProcess("frogpilot_process", "selfdrive.frogpilot.frogpilot_process", always_run), + PythonProcess("mapd", "selfdrive.frogpilot.navigation.mapd", always_run), ] managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/manager/test/__init__.py b/selfdrive/manager/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py new file mode 100755 index 0000000..1ae94b2 --- /dev/null +++ b/selfdrive/manager/test/test_manager.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import os +import pytest +import signal +import time +import unittest + +from parameterized import parameterized + +from cereal import car +from openpilot.common.params import Params +import openpilot.selfdrive.manager.manager as manager +from openpilot.selfdrive.manager.process import ensure_running +from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.hardware import HARDWARE + +os.environ['FAKEUPLOAD'] = "1" + +MAX_STARTUP_TIME = 3 +BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] + + +@pytest.mark.tici +class TestManager(unittest.TestCase): + def setUp(self): + HARDWARE.set_power_save(False) + + # ensure clean CarParams + params = Params() + params.clear_all() + + def tearDown(self): + manager.manager_cleanup() + + def test_manager_prepare(self): + os.environ['PREPAREONLY'] = '1' + manager.main() + + def test_blacklisted_procs(self): + # TODO: ensure there are blacklisted procs until we have a dedicated test + self.assertTrue(len(BLACKLIST_PROCS), "No blacklisted procs to test not_run") + + @parameterized.expand([(i,) for i in range(10)]) + def test_startup_time(self, index): + start = time.monotonic() + os.environ['PREPAREONLY'] = '1' + manager.main() + t = time.monotonic() - start + assert t < MAX_STARTUP_TIME, f"startup took {t}s, expected <{MAX_STARTUP_TIME}s" + + @unittest.skip("this test is flaky the way it's currently written, should be moved to test_onroad") + def test_clean_exit(self): + """ + Ensure all processes exit cleanly when stopped. + """ + HARDWARE.set_power_save(False) + manager.manager_init() + + CP = car.CarParams.new_message() + procs = ensure_running(managed_processes.values(), True, Params(), CP, not_run=BLACKLIST_PROCS) + + time.sleep(10) + + for p in procs: + with self.subTest(proc=p.name): + state = p.get_process_state_msg() + self.assertTrue(state.running, f"{p.name} not running") + exit_code = p.stop(retry=False) + + self.assertNotIn(p.name, BLACKLIST_PROCS, f"{p.name} was started") + + self.assertTrue(exit_code is not None, f"{p.name} failed to exit") + + # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code + exit_codes = [0, 1] + if p.sigkill: + exit_codes = [-signal.SIGKILL] + self.assertIn(exit_code, exit_codes, f"{p.name} died with {exit_code}") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/monitoring/README.md b/selfdrive/monitoring/README.md new file mode 100644 index 0000000..2a29ea0 --- /dev/null +++ b/selfdrive/monitoring/README.md @@ -0,0 +1,15 @@ +# driver monitoring (DM) + +Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle. + +## Troubleshooting + +Before creating a bug report, go through these troubleshooting steps. + +* Ensure the driver-facing camera has a good view of the driver in normal driving positions. + * This can be checked in Settings -> Device -> Preview Driver Camera (when car is off). +* If the camera can't see the driver, the device should be re-mounted. + +## Bug report + +In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive. diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 74e0067..7c1c297 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.stat_live import RunningStatFilter -from openpilot.common.transformations.camera import tici_d_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS EventName = car.CarEvent.EventName @@ -19,19 +19,12 @@ class DRIVER_MONITOR_SETTINGS(): def __init__(self): self._DT_DMON = DT_DMON # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 - # self._AWARENESS_TIME = 30. # passive wheeltouch total timeout - # self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. - # self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. - # self._DISTRACTED_TIME = 11. # active monitoring total timeout - # self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. - # self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. - - self._AWARENESS_TIME = 60. # passive wheeltouch total timeout - self._AWARENESS_PRE_TIME_TILL_TERMINAL = 60. - self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 30. - self._DISTRACTED_TIME = 30. # active monitoring total timeout - self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 60. - self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 30. + self._AWARENESS_TIME = 30. # passive wheeltouch total timeout + self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. + self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. + self._DISTRACTED_TIME = 11. # active monitoring total timeout + self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. + self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.7 self._EYE_THRESHOLD = 0.65 @@ -78,9 +71,11 @@ class DRIVER_MONITOR_SETTINGS(): self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts +# TODO: get these live # model output refers to center of undistorted+leveled image EFL = 598.0 # focal length in K -W, H = tici_d_frame_size # corrected image has same size as raw +cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw +W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw class DistractedType: NOT_DISTRACTED = 0 diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py new file mode 100644 index 0000000..c02d448 --- /dev/null +++ b/selfdrive/monitoring/test_monitoring.py @@ -0,0 +1,214 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np + +from cereal import car, log +from openpilot.common.realtime import DT_DMON +from openpilot.selfdrive.controls.lib.events import Events +from openpilot.selfdrive.monitoring.driver_monitor import DriverStatus, DRIVER_MONITOR_SETTINGS + +EventName = car.CarEvent.EventName +dm_settings = DRIVER_MONITOR_SETTINGS() + +TEST_TIMESPAN = 120 # seconds +DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 +DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1 +INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 +INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 + +def make_msg(face_detected, distracted=False, model_uncertain=False): + ds = log.DriverStateV2.new_message() + ds.leftDriverData.faceOrientation = [0., 0., 0.] + ds.leftDriverData.facePosition = [0., 0.] + ds.leftDriverData.faceProb = 1. * face_detected + ds.leftDriverData.leftEyeProb = 1. + ds.leftDriverData.rightEyeProb = 1. + ds.leftDriverData.leftBlinkProb = 1. * distracted + ds.leftDriverData.rightBlinkProb = 1. * distracted + ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] + ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] + # TODO: test both separately when e2e is used + ds.leftDriverData.readyProb = [0., 0., 0., 0.] + ds.leftDriverData.notReadyProb = [0., 0.] + return ds + + +# driver state from neural net, 10Hz +msg_NO_FACE_DETECTED = make_msg(False) +msg_ATTENTIVE = make_msg(True) +msg_DISTRACTED = make_msg(True, distracted=True) +msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True) +msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True) +msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5) + +# driver interaction with car +car_interaction_DETECTED = True +car_interaction_NOT_DETECTED = False + +# some common state vectors +always_no_face = [msg_NO_FACE_DETECTED] * int(TEST_TIMESPAN / DT_DMON) +always_attentive = [msg_ATTENTIVE] * int(TEST_TIMESPAN / DT_DMON) +always_distracted = [msg_DISTRACTED] * int(TEST_TIMESPAN / DT_DMON) +always_true = [True] * int(TEST_TIMESPAN / DT_DMON) +always_false = [False] * int(TEST_TIMESPAN / DT_DMON) + +# TODO: this only tests DriverStatus +class TestMonitoring(unittest.TestCase): + def _run_seq(self, msgs, interaction, engaged, standstill): + DS = DriverStatus() + events = [] + for idx in range(len(msgs)): + e = Events() + DS.update_states(msgs[idx], [0, 0, 0], 0, engaged[idx]) + # cal_rpy and car_speed don't matter here + + # evaluate events at 10Hz for tests + DS.update_events(e, interaction[idx], engaged[idx], standstill[idx]) + events.append(e) + assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" + return events, DS + + def _assert_no_events(self, events): + self.assertTrue(all(not len(e) for e in events)) + + # engaged, driver is attentive all the time + def test_fully_aware_driver(self): + events, _ = self._run_seq(always_attentive, always_false, always_true, always_false) + self._assert_no_events(events) + + # engaged, driver is distracted and does nothing + def test_fully_distracted_driver(self): + events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) + self.assertEqual(len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]), 0) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + + ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], + EventName.preDriverDistracted) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + + ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME + + ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0], EventName.driverDistracted) + self.assertIs(type(d_status.awareness), float) + + # engaged, no face detected the whole time, no action + def test_fully_invisible_driver(self): + events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) + self.assertTrue(len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0) + self.assertEqual(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + + ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], + EventName.preDriverUnresponsive) + self.assertEqual(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + + ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertEqual(events[int((d_status.settings._AWARENESS_TIME + + ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0], EventName.driverUnresponsive) + + # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel + # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention + def test_normal_driver(self): + ds_vector = [msg_DISTRACTED] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_ATTENTIVE] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_DISTRACTED] * int((DISTRACTED_SECONDS_TO_ORANGE+2)/DT_DMON) + \ + [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) + interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ + [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) + events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + self.assertEqual(len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]), 0) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]), 0) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]), 0) + + # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ + # driver dodges, and then touches wheel to no avail, disengages and reengages + # - orange/red alert should remain after disappearance, and only disengaging clears red + def test_biggest_comma_fan(self): + _invisible_time = 2 # seconds + ds_vector = always_distracted[:] + interaction_vector = always_false[:] + op_vector = always_true[:] + ds_vector[int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] \ + = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + ds_vector[int((DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] \ + = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + interaction_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] \ + = [True] * int(1/DT_DMON) + op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \ + = [False] * int(0.5/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0], EventName.driverDistracted) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0], EventName.driverDistracted) + self.assertTrue(len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0) + + # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears + # - both actions should clear the alert, but momentary appearance should not + def test_sometimes_transparent_commuter(self): + _visible_time = np.random.choice([0.5, 10]) + ds_vector = always_no_face[:]*2 + interaction_vector = always_false[:]*2 + ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ + [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + self.assertTrue(len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0) + if _visible_time == 0.5: + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0], EventName.preDriverUnresponsive) + elif _visible_time == 10: + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0) + + # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages + # - only disengage will clear the alert + def test_last_second_responder(self): + _visible_time = 2 # seconds + ds_vector = always_no_face[:] + interaction_vector = always_false[:] + op_vector = always_true[:] + ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) + op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + self.assertTrue(len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0], EventName.driverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0], EventName.driverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0], EventName.driverUnresponsive) + self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0) + + # disengaged, always distracted driver + # - dm should stay quiet when not engaged + def test_pure_dashcam_user(self): + events, _ = self._run_seq(always_distracted, always_false, always_false, always_false) + self.assertTrue(sum(len(event) for event in events) == 0) + + # engaged, car stops at traffic light, down to orange, no action, then car starts moving + # - should only reach green when stopped, but continues counting down on launch + def test_long_traffic_light_victim(self): + _redlight_time = 60 # seconds + standstill_vector = always_true[:] + standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) + events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0], + EventName.preDriverDistracted) + self.assertEqual(events[int((_redlight_time-0.1)/DT_DMON)].names[0], EventName.preDriverDistracted) + self.assertEqual(events[int((_redlight_time+0.5)/DT_DMON)].names[0], EventName.promptDriverDistracted) + + # engaged, model is somehow uncertain and driver is distracted + # - should fall back to wheel touch after uncertain alert + def test_somehow_indecisive_model(self): + ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) + interaction_vector = always_false[:] + events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + self.assertTrue(EventName.preDriverUnresponsive in + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names) + self.assertTrue(EventName.promptDriverUnresponsive in + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names) + self.assertTrue(EventName.driverUnresponsive in + events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py index 27df060..e3083a0 100644 --- a/selfdrive/sentry.py +++ b/selfdrive/sentry.py @@ -1,7 +1,12 @@ """Install exception handler for process crash.""" import os import sentry_sdk +import socket +import time import traceback +import urllib.request +import urllib.error + from datetime import datetime from enum import Enum from sentry_sdk.integrations.threading import ThreadingIntegration @@ -9,10 +14,9 @@ from sentry_sdk.integrations.threading import ThreadingIntegration from openpilot.common.params import Params from openpilot.system.hardware import HARDWARE, PC from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_branch, get_commit, get_origin, get_short_branch, get_version, is_tested_branch +from openpilot.system.version import get_commit, get_short_branch, get_origin, get_version - -CRASHES_DIR = '/data/community/crashes/' +CRASHES_DIR = "/data/community/crashes/" class SentryProject(Enum): # python project @@ -21,28 +25,143 @@ class SentryProject(Enum): SELFDRIVE_NATIVE = "https://5ad1714d27324c74a30f9c538bff3b8d@o4505034923769856.ingest.sentry.io/4505034930651136" -def report_tombstone(fn: str, message: str, contents: str) -> None: - cloudlog.error({'tombstone': message}) +def sentry_pinged(url="https://sentry.io", timeout=5): + try: + urllib.request.urlopen(url, timeout=timeout) + return True + except (urllib.error.URLError, socket.timeout): + return False - with sentry_sdk.configure_scope() as scope: - scope.set_extra("tombstone_fn", fn) - scope.set_extra("tombstone", contents) - sentry_sdk.capture_message(message=message) - sentry_sdk.flush() + +def bind_user() -> None: + sentry_sdk.set_user({"id": HARDWARE.get_serial()}) + + +def report_tombstone(fn: str, message: str, contents: str) -> None: + FrogPilot = "frogai" in get_origin().lower() + if not FrogPilot or PC: + return + + no_internet = 0 + while True: + if sentry_pinged(): + cloudlog.error({'tombstone': message}) + + with sentry_sdk.configure_scope() as scope: + bind_user() + scope.set_extra("tombstone_fn", fn) + scope.set_extra("tombstone", contents) + sentry_sdk.capture_message(message=message) + sentry_sdk.flush() + break + else: + if no_internet > 5: + break + no_internet += 1 + time.sleep(600) + + +def chunk_data(data, size): + return [data[i:i+size] for i in range(0, len(data), size)] + + +def format_params(params): + formatted_params = [] + for k, v in params.items(): + if isinstance(v, bytes): + param_value = format(float(v), '.12g') if v.replace(b'.', b'').isdigit() else v.decode() + elif isinstance(v, float): + param_value = format(v, '.12g') + else: + param_value = v + formatted_params.append(f"{k}: {param_value}") + return formatted_params + + +def get_frogpilot_params(params, keys): + return {key: params.get(key) or '0' for key in keys} + + +def set_sentry_scope(scope, chunks, label): + scope.set_extra(label, '\n'.join(['\n'.join(chunk) for chunk in chunks])) + + +def capture_fingerprint(params, candidate, blocked=False): + bind_user() + + control_keys, vehicle_keys, visual_keys, other_keys, tracking_keys = [ + "AlwaysOnLateral", "AlwaysOnLateralMain", "HideAOLStatusBar", "ConditionalExperimental", "CESpeed", "CESpeedLead", "CECurves", "CECurvesLead", + "CENavigation", "CENavigationIntersections", "CENavigationTurns", "CENavigationLead", "CESlowerLead", "CEStopLights", "CEStopLightsLead", + "CESignal", "HideCEMStatusBar", "CustomPersonalities", "TrafficFollow", "TrafficJerk", "AggressiveFollow", "AggressiveJerk", "StandardFollow", + "StandardJerk", "RelaxedFollow", "RelaxedJerk", "DeviceManagement", "IncreaseThermalLimits", "DeviceShutdown", "NoLogging", "NoUploads", "LowVoltageShutdown", + "OfflineMode", "ExperimentalModeActivation", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap", "ExperimentalModeViaDistance", "LateralTune", + "ForceAutoTune", "NNFF", "NNFFLite", "SteerRatio", "TacoTune", "TurnDesires", "SteerRatio", "LongitudinalTune", "AccelerationProfile", "DecelerationProfile", + "AggressiveAcceleration", "StoppingDistance", "LeadDetectionThreshold", "SmoothBraking", "SmoothBrakingFarLead", "SmoothBrakingJerk", "TrafficMode", + "MTSCEnabled", "DisableMTSCSmoothing", "MTSCCurvatureCheck", "MTSCAggressiveness", "ModelSelector", "Model", "NudgelessLaneChange", "LaneChangeTime", + "LaneDetectionWidth", "OneLaneChange", "LaneDetectionWidth", "QOLControls", "CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "HigherBitrate", + "OnroadDistanceButton", "KaofuiIcons", "PauseLateralSpeed", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset", "SpeedLimitController", "Offset1", + "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority", "SLCConfirmation", "SLCConfirmationLower", "SLCConfirmationHigher", + "ForceMPHDashboard", "SLCLookaheadHigher", "SLCLookaheadLower", "SetSpeedLimit", "ShowSLCOffset", "ShowSLCOffsetUI", "UseVienna", "VisionTurnControl", + "DisableVTSCSmoothing", "CurveSensitivity", "TurnAggressiveness", + ], [ + "ForceFingerprint", "DisableOpenpilotLongitudinal", "EVTable", "LongPitch", "GasRegenCmd", "CrosstrekTorque", "LockDoors", "StockTune", "CydiaTune", + "DragonPilotTune", "FrogsGoMooTune", "LockDoors", "SNGHack", + ], [ + "AlertVolumeControl", "DisengageVolume", "EngageVolume", "PromptVolume", "PromptDistractedVolume", "RefuseVolume", "WarningSoftVolume", + "WarningImmediateVolume", "CustomAlerts", "GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert", "SpeedLimitChangedAlert", "CustomUI", + "Compass", "DeveloperUI", "ShowJerk", "LeadInfo", "ShowTuning", "UseSI", "FPSCounter", "CustomPaths", "AccelerationPath", "AdjacentPath", "BlindSpotPath", + "AdjacentPathMetrics", "PedalsOnUI", "RoadNameUI", "WheelIcon", "RotatingWheel", "CustomTheme", "CustomColors", "CustomIcons", "CustomSignals", "CustomSounds", + "GoatScream", "HolidayThemes", "RandomEvents", "ModelUI", "DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", + "UnlimitedLength", "QOLVisuals", "BigMap", "FullMap", "CameraView", "DriverCamera", "HideSpeed", "HideSpeedUI", "MapStyle", "NumericalTemp", "Fahrenheit", + "WheelSpeed", "ScreenManagement", "HideUIElements", "HideAlerts", "HideMapIcon", "HideMaxSpeed", "ScreenBrightness", "ScreenBrightnessOnroad", "ScreenRecorder", + "ScreenTimeout", "ScreenTimeoutOnroad", "StandbyMode", + ], [ + "AutomaticUpdates", "ShowCPU", "ShowGPU", "ShowIP", "ShowMemoryUsage", "ShowStorageLeft", "ShowStorageUsed", "Sidebar", "TetheringEnabled", + ], [ + "FrogPilotDrives", "FrogPilotKilometers", "FrogPilotMinutes" + ] + + control_params, vehicle_params, visual_params, other_params, tracking_params = map(lambda keys: get_frogpilot_params(params, keys), [control_keys, vehicle_keys, visual_keys, other_keys, tracking_keys]) + control_values, vehicle_values, visual_values, other_values, tracking_values = map(format_params, [control_params, vehicle_params, visual_params, other_params, tracking_params]) + control_chunks, vehicle_chunks, visual_chunks, other_chunks, tracking_chunks = map(lambda data: chunk_data(data, 50), [control_values, vehicle_values, visual_values, other_values, tracking_values]) + + no_internet = 0 + while True: + if sentry_pinged(): + for chunks, label in zip([control_chunks, vehicle_chunks, visual_chunks, other_chunks, tracking_chunks], ["FrogPilot Controls", "FrogPilot Vehicles", "FrogPilot Visuals", "Other Toggles", "FrogPilot Tracking"]): + with sentry_sdk.configure_scope() as scope: + set_sentry_scope(scope, chunks, label) + if blocked: + sentry_sdk.capture_message("Blocked user from using the development branch", level='error') + else: + sentry_sdk.capture_message("Fingerprinted %s" % candidate, level='info') + params.put_bool("FingerprintLogged", True) + sentry_sdk.flush() + break + else: + if no_internet > 5: + break + no_internet += 1 + time.sleep(600) def capture_exception(*args, **kwargs) -> None: save_exception(traceback.format_exc()) cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) + FrogPilot = "frogai" in get_origin().lower() + if not FrogPilot or PC: + return + try: + bind_user() sentry_sdk.capture_exception(*args, **kwargs) sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 except Exception: cloudlog.exception("sentry exception") -def save_exception(exc_text): +def save_exception(exc_text: str) -> None: if not os.path.exists(CRASHES_DIR): os.makedirs(CRASHES_DIR) @@ -53,20 +172,13 @@ def save_exception(exc_text): for file in files: with open(file, 'w') as f: - f.write(exc_text) + if file.endswith("error.txt"): + lines = exc_text.splitlines()[-10:] + f.write("\n".join(lines)) + else: + f.write(exc_text) - -def bind_user(**kwargs) -> None: - sentry_sdk.set_user(kwargs) - sentry_sdk.flush() - - -def capture_warning(warning_string, serial_id): - with sentry_sdk.configure_scope() as scope: - scope.fingerprint = [warning_string, serial_id] - bind_user(id=serial_id) - sentry_sdk.capture_message(warning_string, level='info') - sentry_sdk.flush() + print('Logged current crash to {}'.format(files)) def set_tag(key: str, value: str) -> None: @@ -74,10 +186,9 @@ def set_tag(key: str, value: str) -> None: def init(project: SentryProject) -> bool: - # forks like to mess with this, so double check - frogpilot = "frogai" in get_origin().lower() - if not frogpilot or PC: - return False + params = Params() + installed = params.get("InstallDate", encoding='utf-8') + updated = params.get("Updated", encoding='utf-8') short_branch = get_short_branch() @@ -90,10 +201,6 @@ def init(project: SentryProject) -> bool: else: env = short_branch - params = Params() - installed = params.get("InstallDate", encoding='utf-8') - updated = params.get("Updated", encoding='utf-8') - integrations = [] if project == SentryProject.SELFDRIVE: integrations.append(ThreadingIntegration(propagate_hub=True)) @@ -104,14 +211,14 @@ def init(project: SentryProject) -> bool: integrations=integrations, traces_sample_rate=1.0, max_value_length=8192, - environment=env, - send_default_pii=True) + environment=env) sentry_sdk.set_user({"id": HARDWARE.get_serial()}) - sentry_sdk.set_tag("branch", get_branch()) + sentry_sdk.set_tag("branch", short_branch) sentry_sdk.set_tag("commit", get_commit()) sentry_sdk.set_tag("updated", updated) sentry_sdk.set_tag("installed", installed) + sentry_sdk.set_tag("repo", get_origin()) if project == SentryProject.SELFDRIVE: sentry_sdk.Hub.current.start_session() diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index 94572b8..299aa29 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -5,7 +5,7 @@ import time from pathlib import Path from collections import defaultdict from datetime import datetime, timezone -from typing import NoReturn, Union, List, Dict +from typing import NoReturn from openpilot.common.params import Params from cereal.messaging import SubMaster @@ -61,7 +61,7 @@ class StatLog: def main() -> NoReturn: dongle_id = Params().get("DongleId", encoding='utf-8') - def get_influxdb_line(measurement: str, value: Union[float, Dict[str, float]], timestamp: datetime, tags: dict) -> str: + def get_influxdb_line(measurement: str, value: float | dict[str, float], timestamp: datetime, tags: dict) -> str: res = f"{measurement}" for k, v in tags.items(): res += f",{k}={str(v)}" @@ -102,7 +102,7 @@ def main() -> NoReturn: idx = 0 last_flush_time = time.monotonic() gauges = {} - samples: Dict[str, List[float]] = defaultdict(list) + samples: dict[str, list[float]] = defaultdict(list) try: while True: started_prev = sm['deviceState'].started diff --git a/selfdrive/test/.gitignore b/selfdrive/test/.gitignore new file mode 100644 index 0000000..5801faa --- /dev/null +++ b/selfdrive/test/.gitignore @@ -0,0 +1,9 @@ +out/ +docker_out/ + +process_replay/diff.txt +process_replay/model_diff.txt +valgrind_logs.txt + +*.bz2 +*.hevc diff --git a/selfdrive/test/__init__.py b/selfdrive/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/test/ci_shell.sh b/selfdrive/test/ci_shell.sh new file mode 100644 index 0000000..a5ff714 --- /dev/null +++ b/selfdrive/test/ci_shell.sh @@ -0,0 +1,19 @@ +#!/bin/bash -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +OP_ROOT="$DIR/../../" + +if [ -z "$BUILD" ]; then + docker pull ghcr.io/commaai/openpilot-base:latest +else + docker build --cache-from ghcr.io/commaai/openpilot-base:latest -t ghcr.io/commaai/openpilot-base:latest -f $OP_ROOT/Dockerfile.openpilot_base . +fi + +docker run \ + -it \ + --rm \ + --volume $OP_ROOT:$OP_ROOT \ + --workdir $PWD \ + --env PYTHONPATH=$OP_ROOT \ + ghcr.io/commaai/openpilot-base:latest \ + /bin/bash diff --git a/selfdrive/test/ciui.py b/selfdrive/test/ciui.py new file mode 100644 index 0000000..f3b0c1a --- /dev/null +++ b/selfdrive/test/ciui.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +import signal +import subprocess + +signal.signal(signal.SIGINT, signal.SIG_DFL) +signal.signal(signal.SIGTERM, signal.SIG_DFL) + +from PyQt5.QtCore import QTimer +from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QLabel +from openpilot.selfdrive.ui.qt.python_helpers import set_main_window + +class Window(QWidget): + def __init__(self, parent=None): + super().__init__(parent) + + layout = QVBoxLayout() + self.setLayout(layout) + + self.l = QLabel("jenkins runner") + layout.addWidget(self.l) + layout.addStretch(1) + layout.setContentsMargins(20, 20, 20, 20) + + cmds = [ + "cat /etc/hostname", + "echo AGNOS v$(cat /VERSION)", + "uptime -p", + ] + self.labels = {} + for c in cmds: + self.labels[c] = QLabel(c) + layout.addWidget(self.labels[c]) + + self.setStyleSheet(""" + * { + color: white; + font-size: 55px; + background-color: black; + font-family: "JetBrains Mono"; + } + """) + + self.timer = QTimer() + self.timer.timeout.connect(self.update) + self.timer.start(10 * 1000) + self.update() + + def update(self): + for cmd, label in self.labels.items(): + out = subprocess.run(cmd, capture_output=True, + shell=True, check=False, encoding='utf8').stdout + label.setText(out.strip()) + +if __name__ == "__main__": + app = QApplication([]) + w = Window() + set_main_window(w) + app.exec_() diff --git a/selfdrive/test/cpp_harness.py b/selfdrive/test/cpp_harness.py new file mode 100644 index 0000000..f9d3e68 --- /dev/null +++ b/selfdrive/test/cpp_harness.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 +import subprocess +import sys + +from openpilot.common.prefix import OpenpilotPrefix + + +with OpenpilotPrefix(): + ret = subprocess.call(sys.argv[1:]) + +exit(ret) diff --git a/selfdrive/test/docker_build.sh b/selfdrive/test/docker_build.sh new file mode 100644 index 0000000..5f77ceb --- /dev/null +++ b/selfdrive/test/docker_build.sh @@ -0,0 +1,26 @@ +#!/usr/bin/env bash +set -e + +# To build sim and docs, you can run the following to mount the scons cache to the same place as in CI: +# mkdir -p .ci_cache/scons_cache +# sudo mount --bind /tmp/scons_cache/ .ci_cache/scons_cache + +SCRIPT_DIR=$(dirname "$0") +OPENPILOT_DIR=$SCRIPT_DIR/../../ +if [ -n "$TARGET_ARCHITECTURE" ]; then + PLATFORM="linux/$TARGET_ARCHITECTURE" + TAG_SUFFIX="-$TARGET_ARCHITECTURE" +else + PLATFORM="linux/$(uname -m)" + TAG_SUFFIX="" +fi + +source $SCRIPT_DIR/docker_common.sh $1 "$TAG_SUFFIX" + +DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load --cache-to type=inline --cache-from type=registry,ref=$REMOTE_TAG -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR + +if [ -n "$PUSH_IMAGE" ]; then + docker push $REMOTE_TAG + docker tag $REMOTE_TAG $REMOTE_SHA_TAG + docker push $REMOTE_SHA_TAG +fi diff --git a/selfdrive/test/docker_common.sh b/selfdrive/test/docker_common.sh new file mode 100644 index 0000000..f8a4237 --- /dev/null +++ b/selfdrive/test/docker_common.sh @@ -0,0 +1,21 @@ +if [ "$1" = "base" ]; then + export DOCKER_IMAGE=openpilot-base + export DOCKER_FILE=Dockerfile.openpilot_base +elif [ "$1" = "sim" ]; then + export DOCKER_IMAGE=openpilot-sim + export DOCKER_FILE=tools/sim/Dockerfile.sim +elif [ "$1" = "prebuilt" ]; then + export DOCKER_IMAGE=openpilot-prebuilt + export DOCKER_FILE=Dockerfile.openpilot +else + echo "Invalid docker build image: '$1'" + exit 1 +fi + +export DOCKER_REGISTRY=ghcr.io/commaai +export COMMIT_SHA=$(git rev-parse HEAD) + +TAG_SUFFIX=$2 +LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX +REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG +REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA diff --git a/selfdrive/test/docker_tag_multiarch.sh b/selfdrive/test/docker_tag_multiarch.sh new file mode 100644 index 0000000..c176180 --- /dev/null +++ b/selfdrive/test/docker_tag_multiarch.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash +set -e + +if [ $# -lt 2 ]; then + echo "Usage: $0 ..." + exit 1 +fi + +SCRIPT_DIR=$(dirname "$0") +ARCHS=("${@:2}") + +source $SCRIPT_DIR/docker_common.sh $1 + +MANIFEST_AMENDS="" +for ARCH in ${ARCHS[@]}; do + MANIFEST_AMENDS="$MANIFEST_AMENDS --amend $REMOTE_TAG-$ARCH:$COMMIT_SHA" +done + +docker manifest create $REMOTE_TAG $MANIFEST_AMENDS +docker manifest create $REMOTE_SHA_TAG $MANIFEST_AMENDS + +if [[ -n "$PUSH_IMAGE" ]]; then + docker manifest push $REMOTE_TAG + docker manifest push $REMOTE_SHA_TAG +fi diff --git a/selfdrive/test/fuzzy_generation.py b/selfdrive/test/fuzzy_generation.py new file mode 100644 index 0000000..26c35c0 --- /dev/null +++ b/selfdrive/test/fuzzy_generation.py @@ -0,0 +1,85 @@ +import capnp +import hypothesis.strategies as st +from typing import Any +from collections.abc import Callable + +from cereal import log + +DrawType = Callable[[st.SearchStrategy], Any] + + +class FuzzyGenerator: + def __init__(self, draw: DrawType, real_floats: bool): + self.draw = draw + self.real_floats = real_floats + + def generate_native_type(self, field: str) -> st.SearchStrategy[bool | int | float | str | bytes]: + def floats(**kwargs) -> st.SearchStrategy[float]: + allow_nan = not self.real_floats + allow_infinity = not self.real_floats + return st.floats(**kwargs, allow_nan=allow_nan, allow_infinity=allow_infinity) + + if field == 'bool': + return st.booleans() + elif field == 'int8': + return st.integers(min_value=-2**7, max_value=2**7-1) + elif field == 'int16': + return st.integers(min_value=-2**15, max_value=2**15-1) + elif field == 'int32': + return st.integers(min_value=-2**31, max_value=2**31-1) + elif field == 'int64': + return st.integers(min_value=-2**63, max_value=2**63-1) + elif field == 'uint8': + return st.integers(min_value=0, max_value=2**8-1) + elif field == 'uint16': + return st.integers(min_value=0, max_value=2**16-1) + elif field == 'uint32': + return st.integers(min_value=0, max_value=2**32-1) + elif field == 'uint64': + return st.integers(min_value=0, max_value=2**64-1) + elif field == 'float32': + return floats(width=32) + elif field == 'float64': + return floats(width=64) + elif field == 'text': + return st.text(max_size=1000) + elif field == 'data': + return st.binary(max_size=1000) + elif field == 'anyPointer': + return st.text() + else: + raise NotImplementedError(f'Invalid type : {field}') + + def generate_field(self, field: capnp.lib.capnp._StructSchemaField) -> st.SearchStrategy: + def rec(field_type: capnp.lib.capnp._DynamicStructReader) -> st.SearchStrategy: + if field_type.which() == 'struct': + return self.generate_struct(field.schema.elementType if base_type == 'list' else field.schema) + elif field_type.which() == 'list': + return st.lists(rec(field_type.list.elementType)) + elif field_type.which() == 'enum': + schema = field.schema.elementType if base_type == 'list' else field.schema + return st.sampled_from(list(schema.enumerants.keys())) + else: + return self.generate_native_type(field_type.which()) + + if 'slot' in field.proto.to_dict(): + base_type = field.proto.slot.type.which() + return rec(field.proto.slot.type) + else: + return self.generate_struct(field.schema) + + def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: str = None) -> st.SearchStrategy[dict[str, Any]]: + full_fill: list[str] = list(schema.non_union_fields) + single_fill: list[str] = [event] if event else [self.draw(st.sampled_from(schema.union_fields))] if schema.union_fields else [] + return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in full_fill + single_fill}) + + @classmethod + def get_random_msg(cls, draw: DrawType, struct: capnp.lib.capnp._StructModule, real_floats: bool = False) -> dict[str, Any]: + fg = cls(draw, real_floats=real_floats) + data: dict[str, Any] = draw(fg.generate_struct(struct.schema)) + return data + + @classmethod + def get_random_event_msg(cls, draw: DrawType, events: list[str], real_floats: bool = False) -> list[dict[str, Any]]: + fg = cls(draw, real_floats=real_floats) + return [draw(fg.generate_struct(log.Event.schema, e)) for e in sorted(events)] diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py new file mode 100644 index 0000000..fe47637 --- /dev/null +++ b/selfdrive/test/helpers.py @@ -0,0 +1,124 @@ +import contextlib +import http.server +import os +import threading +import time + +from functools import wraps + +import cereal.messaging as messaging +from openpilot.common.params import Params +from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.hardware import PC +from openpilot.system.version import training_version, terms_version + + +def set_params_enabled(): + os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" + os.environ['LOGPRINT'] = "debug" + + params = Params() + params.put("HasAcceptedTerms", terms_version) + params.put("CompletedTrainingVersion", training_version) + params.put_bool("OpenpilotEnabledToggle", True) + + # valid calib + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = 20 + msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] + params.put("CalibrationParams", msg.to_bytes()) + +def phone_only(f): + @wraps(f) + def wrap(self, *args, **kwargs): + if PC: + self.skipTest("This test is not meant to run on PC") + f(self, *args, **kwargs) + return wrap + +def release_only(f): + @wraps(f) + def wrap(self, *args, **kwargs): + if "RELEASE" not in os.environ: + self.skipTest("This test is only for release branches") + f(self, *args, **kwargs) + return wrap + + +@contextlib.contextmanager +def processes_context(processes, init_time=0, ignore_stopped=None): + ignore_stopped = [] if ignore_stopped is None else ignore_stopped + + # start and assert started + for n, p in enumerate(processes): + managed_processes[p].start() + if n < len(processes) - 1: + time.sleep(init_time) + + assert all(managed_processes[name].proc.exitcode is None for name in processes) + + try: + yield [managed_processes[name] for name in processes] + # assert processes are still started + assert all(managed_processes[name].proc.exitcode is None for name in processes if name not in ignore_stopped) + finally: + for p in processes: + managed_processes[p].stop() + + +def with_processes(processes, init_time=0, ignore_stopped=None): + def wrapper(func): + @wraps(func) + def wrap(*args, **kwargs): + with processes_context(processes, init_time, ignore_stopped): + return func(*args, **kwargs) + + return wrap + return wrapper + + +def noop(*args, **kwargs): + pass + + +def read_segment_list(segment_list_path): + with open(segment_list_path) as f: + seg_list = f.read().splitlines() + + return [(platform[2:], segment) for platform, segment in zip(seg_list[::2], seg_list[1::2], strict=True)] + + +@contextlib.contextmanager +def http_server_context(handler, setup=None): + host = '127.0.0.1' + server = http.server.HTTPServer((host, 0), handler) + port = server.server_port + t = threading.Thread(target=server.serve_forever) + t.start() + + if setup is not None: + setup(host, port) + + try: + yield (host, port) + finally: + server.shutdown() + server.server_close() + t.join() + + +def with_http_server(func, handler=http.server.BaseHTTPRequestHandler, setup=None): + @wraps(func) + def inner(*args, **kwargs): + with http_server_context(handler, setup) as (host, port): + return func(*args, f"http://{host}:{port}", **kwargs) + return inner + + +def DirectoryHttpServer(directory) -> type[http.server.SimpleHTTPRequestHandler]: + # creates an http server that serves files from directory + class Handler(http.server.SimpleHTTPRequestHandler): + def __init__(self, *args, **kwargs): + super().__init__(*args, directory=str(directory), **kwargs) + + return Handler diff --git a/selfdrive/test/longitudinal_maneuvers/.gitignore b/selfdrive/test/longitudinal_maneuvers/.gitignore new file mode 100644 index 0000000..d42ab35 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/.gitignore @@ -0,0 +1 @@ +out/* diff --git a/selfdrive/test/longitudinal_maneuvers/__init__.py b/selfdrive/test/longitudinal_maneuvers/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py new file mode 100644 index 0000000..6c8495c --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -0,0 +1,73 @@ +import numpy as np +from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant + + +class Maneuver: + def __init__(self, title, duration, **kwargs): + # Was tempted to make a builder class + self.distance_lead = kwargs.get("initial_distance_lead", 200.0) + self.speed = kwargs.get("initial_speed", 0.0) + self.lead_relevancy = kwargs.get("lead_relevancy", 0) + + self.breakpoints = kwargs.get("breakpoints", [0.0, duration]) + self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))]) + self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))]) + self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))]) + + self.only_lead2 = kwargs.get("only_lead2", False) + self.only_radar = kwargs.get("only_radar", False) + self.ensure_start = kwargs.get("ensure_start", False) + self.enabled = kwargs.get("enabled", True) + self.e2e = kwargs.get("e2e", False) + self.personality = kwargs.get("personality", 0) + self.force_decel = kwargs.get("force_decel", False) + + self.duration = duration + self.title = title + + def evaluate(self): + plant = Plant( + lead_relevancy=self.lead_relevancy, + speed=self.speed, + distance_lead=self.distance_lead, + enabled=self.enabled, + only_lead2=self.only_lead2, + only_radar=self.only_radar, + e2e=self.e2e, + personality=self.personality, + force_decel=self.force_decel, + ) + + valid = True + logs = [] + while plant.current_time < self.duration: + speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) + prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) + cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values) + log = plant.step(speed_lead, prob, cruise) + + d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. + v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. + log['d_rel'] = d_rel + log['v_rel'] = v_rel + logs.append(np.array([plant.current_time, + log['distance'], + log['distance_lead'], + log['speed'], + speed_lead, + log['acceleration']])) + + if d_rel < .4 and (self.only_radar or prob > 0.5): + print("Crashed!!!!") + valid = False + + if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: + print('LongitudinalPlanner not starting!') + valid = False + if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04: + print('Not stopping with force decel') + valid = False + + + print("maneuver end", valid) + return valid, np.array(logs) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py new file mode 100644 index 0000000..3fb8b6b --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +import time +import numpy as np + +from cereal import log +import cereal.messaging as messaging +from openpilot.common.realtime import Ratekeeper, DT_MDL +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU + + +class Plant: + messaging_initialized = False + + def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, + enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False): + self.rate = 1. / DT_MDL + + if not Plant.messaging_initialized: + Plant.radar = messaging.pub_sock('radarState') + Plant.controls_state = messaging.pub_sock('controlsState') + Plant.car_state = messaging.pub_sock('carState') + Plant.plan = messaging.sub_sock('longitudinalPlan') + Plant.messaging_initialized = True + + self.v_lead_prev = 0.0 + + self.distance = 0. + self.speed = speed + self.acceleration = 0.0 + self.speeds = [] + + # lead car + self.lead_relevancy = lead_relevancy + self.distance_lead = distance_lead + self.enabled = enabled + self.only_lead2 = only_lead2 + self.only_radar = only_radar + self.e2e = e2e + self.personality = personality + self.force_decel = force_decel + + self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) + self.ts = 1. / self.rate + time.sleep(0.1) + self.sm = messaging.SubMaster(['longitudinalPlan']) + + from openpilot.selfdrive.car.honda.values import CAR + from openpilot.selfdrive.car.honda.interface import CarInterface + + self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed) + + @property + def current_time(self): + return float(self.rk.frame) / self.rate + + def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): + # ******** publish a fake model going straight and fake calibration ******** + # note that this is worst case for MPC, since model will delay long mpc by one time step + radar = messaging.new_message('radarState') + control = messaging.new_message('controlsState') + car_state = messaging.new_message('carState') + model = messaging.new_message('modelV2') + a_lead = (v_lead - self.v_lead_prev)/self.ts + self.v_lead_prev = v_lead + + if self.lead_relevancy: + d_rel = np.maximum(0., self.distance_lead - self.distance) + v_rel = v_lead - self.speed + if self.only_radar: + status = True + elif prob > .5: + status = True + else: + status = False + else: + d_rel = 200. + v_rel = 0. + prob = 0.0 + status = False + + lead = log.RadarState.LeadData.new_message() + lead.dRel = float(d_rel) + lead.yRel = float(0.0) + lead.vRel = float(v_rel) + lead.aRel = float(a_lead - self.acceleration) + lead.vLead = float(v_lead) + lead.vLeadK = float(v_lead) + lead.aLeadK = float(a_lead) + # TODO use real radard logic for this + lead.aLeadTau = float(_LEAD_ACCEL_TAU) + lead.status = status + lead.modelProb = float(prob) + if not self.only_lead2: + radar.radarState.leadOne = lead + radar.radarState.leadTwo = lead + + # Simulate model predicting slightly faster speed + # this is to ensure lead policy is effective when model + # does not predict slowdown in e2e mode + position = log.XYZTData.new_message() + position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] + model.modelV2.position = position + velocity = log.XYZTData.new_message() + velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + model.modelV2.velocity = velocity + acceleration = log.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] + model.modelV2.acceleration = acceleration + + control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off + control.controlsState.vCruise = float(v_cruise * 3.6) + control.controlsState.experimentalMode = self.e2e + control.controlsState.personality = self.personality + control.controlsState.forceDecel = self.force_decel + car_state.carState.vEgo = float(self.speed) + car_state.carState.standstill = self.speed < 0.01 + + # ******** get controlsState messages for plotting *** + sm = {'radarState': radar.radarState, + 'carState': car_state.carState, + 'controlsState': control.controlsState, + 'modelV2': model.modelV2} + self.planner.update(sm) + self.speed = self.planner.v_desired_filter.x + self.acceleration = self.planner.a_desired + self.speeds = self.planner.v_desired_trajectory.tolist() + fcw = self.planner.fcw + self.distance_lead = self.distance_lead + v_lead * self.ts + + # ******** run the car ******** + #print(self.distance, speed) + if self.speed <= 0: + self.speed = 0 + self.acceleration = 0 + self.distance = self.distance + self.speed * self.ts + + # *** radar model *** + if self.lead_relevancy: + d_rel = np.maximum(0., self.distance_lead - self.distance) + v_rel = v_lead - self.speed + else: + d_rel = 200. + v_rel = 0. + + # print at 5hz + # if (self.rk.frame % (self.rate // 5)) == 0: + # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" + # % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) + + + # ******** update prevs ******** + self.rk.monitor_time() + + return { + "distance": self.distance, + "speed": self.speed, + "acceleration": self.acceleration, + "speeds": self.speeds, + "distance_lead": self.distance_lead, + "fcw": fcw, + } + +# simple engage in standalone mode +def plant_thread(): + plant = Plant() + while 1: + plant.step() + + +if __name__ == "__main__": + plant_thread() diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py new file mode 100644 index 0000000..713b780 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +import itertools +import unittest +from parameterized import parameterized_class + +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE +from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + + +# TODO: make new FCW tests +def create_maneuvers(kwargs): + maneuvers = [ + Maneuver( + 'approach stopped car at 25m/s, initial distance: 120m', + duration=20., + initial_speed=25., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[30., 0.], + breakpoints=[0., 1.], + **kwargs, + ), + Maneuver( + 'approach stopped car at 20m/s, initial distance 90m', + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=90., + speed_lead_values=[20., 0.], + breakpoints=[0., 1.], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 35.0], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 25.0], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 21.66], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', + duration=40., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + prob_lead_values=[0., 1., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[2., 2.01, 8.8], + **kwargs, + ), + Maneuver( + "approach stopped car at 20m/s, with prob_lead_values", + duration=30., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_lead_values=[0.0, 0., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[0.0, 2., 2.01], + **kwargs, + ), + Maneuver( + "approach slower cut-in car at 20m/s", + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=50., + speed_lead_values=[15., 15.], + breakpoints=[1., 11.], + only_lead2=True, + **kwargs, + ), + Maneuver( + "stay stopped behind radar override lead", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[0., 0.], + prob_lead_values=[0., 0.], + breakpoints=[1., 11.], + only_radar=True, + **kwargs, + ), + Maneuver( + "NaN recovery", + duration=30., + initial_speed=15., + lead_relevancy=True, + initial_distance_lead=60., + speed_lead_values=[0., 0., 0.0], + breakpoints=[1., 1.01, 11.], + cruise_values=[float("nan"), 15., 15.], + **kwargs, + ), + Maneuver( + 'cruising at 25 m/s while disabled', + duration=20., + initial_speed=25., + lead_relevancy=False, + enabled=False, + **kwargs, + ), + ] + if not kwargs['force_decel']: + # controls relies on planner commanding to move for stock-ACC resume spamming + maneuvers.append(Maneuver( + "resume from a stop", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=STOP_DISTANCE, + speed_lead_values=[0., 0., 2.], + breakpoints=[1., 10., 15.], + ensure_start=True, + **kwargs, + )) + return maneuvers + + +@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2)) +class LongitudinalControl(unittest.TestCase): + e2e: bool + force_decel: bool + + def test_maneuver(self): + for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}): + with self.subTest(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel): + print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') + valid, _ = maneuver.evaluate() + self.assertTrue(valid) + + +if __name__ == "__main__": + unittest.main(failfast=True) diff --git a/selfdrive/test/loop_until_fail.sh b/selfdrive/test/loop_until_fail.sh new file mode 100644 index 0000000..b73009d --- /dev/null +++ b/selfdrive/test/loop_until_fail.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -e + +# Loop something forever until it fails, for verifying new tests + +while true; do + $@ +done diff --git a/selfdrive/test/process_replay/.gitignore b/selfdrive/test/process_replay/.gitignore new file mode 100644 index 0000000..63c37e6 --- /dev/null +++ b/selfdrive/test/process_replay/.gitignore @@ -0,0 +1,2 @@ +fakedata/ +debayer_diff.txt diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md new file mode 100644 index 0000000..008a901 --- /dev/null +++ b/selfdrive/test/process_replay/README.md @@ -0,0 +1,126 @@ +# Process replay + +Process replay is a regression test designed to identify any changes in the output of a process. This test replays a segment through individual processes and compares the output to a known good replay. Each make is represented in the test with a segment. + +If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated. + +Use `test_processes.py` to run the test locally. +Use `FILEREADER_CACHE='1' test_processes.py` to cache log files. + +Currently the following processes are tested: + +* controlsd +* radard +* plannerd +* calibrationd +* dmonitoringd +* locationd +* paramsd +* ubloxd +* torqued + +### Usage +``` +Usage: test_processes.py [-h] [--whitelist-procs PROCS] [--whitelist-cars CARS] [--blacklist-procs PROCS] + [--blacklist-cars CARS] [--ignore-fields FIELDS] [--ignore-msgs MSGS] [--update-refs] [--upload-only] +Regression test to identify changes in a process's output +optional arguments: + -h, --help show this help message and exit + --whitelist-procs PROCS Whitelist given processes from the test (e.g. controlsd) + --whitelist-cars WHITELIST_CARS Whitelist given cars from the test (e.g. HONDA) + --blacklist-procs BLACKLIST_PROCS Blacklist given processes from the test (e.g. controlsd) + --blacklist-cars BLACKLIST_CARS Blacklist given cars from the test (e.g. HONDA) + --ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. carState.events) + --ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. onroadEvents) + --update-refs Updates reference logs using current commit + --upload-only Skips testing processes and uploads logs from previous test run +``` + +## Forks + +openpilot forks can use this test with their own reference logs, by default `test_proccess.py` saves logs locally. + +To generate new logs: + +`./test_processes.py` + +Then, check in the new logs using git-lfs. Make sure to also update the `ref_commit` file to the current commit. + +## API + +Process replay test suite exposes programmatic APIs for simultaneously running processes or groups of processes on provided logs. + +```py +def replay_process_with_name(name: Union[str, Iterable[str]], lr: LogIterable, *args, **kwargs) -> List[capnp._DynamicStructReader]: + +def replay_process( + cfg: Union[ProcessConfig, Iterable[ProcessConfig]], lr: LogIterable, frs: Optional[Dict[str, Any]] = None, + fingerprint: Optional[str] = None, return_all_logs: bool = False, custom_params: Optional[Dict[str, Any]] = None, disable_progress: bool = False +) -> List[capnp._DynamicStructReader]: +``` + +Example usage: +```py +from openpilot.selfdrive.test.process_replay import replay_process_with_name +from openpilot.tools.lib.logreader import LogReader + +lr = LogReader(...) + +# provide a name of the process to replay +output_logs = replay_process_with_name('locationd', lr) + +# or list of names +output_logs = replay_process_with_name(['ubloxd', 'locationd'], lr) +``` + +Supported processes: +* controlsd +* radard +* plannerd +* calibrationd +* dmonitoringd +* locationd +* paramsd +* ubloxd +* torqued +* modeld +* dmonitoringmodeld + +Certain processes may require an initial state, which is usually supplied within `Params` and persisting from segment to segment (e.g CalibrationParams, LiveParameters). The `custom_params` is dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files. + +```py +from openpilot.selfdrive.test.process_replay import get_custom_params_from_lr + +previous_segment_lr = LogReader(...) +current_segment_lr = LogReader(...) + +custom_params = get_custom_params_from_lr(previous_segment_lr, 'last') + +output_logs = replay_process_with_name('calibrationd', lr, custom_params=custom_params) +``` + +Replaying processes that use VisionIPC (e.g. modeld, dmonitoringmodeld) require additional `frs` dictionary with camera states as keys and `FrameReader` objects as values. + +```py +from openpilot.tools.lib.framereader import FrameReader + +frs = { + 'roadCameraState': FrameReader(...), + 'wideRoadCameraState': FrameReader(...), + 'driverCameraState': FrameReader(...), +} + +output_logs = replay_process_with_name(['modeld', 'dmonitoringmodeld'], lr, frs=frs) +``` + +To capture stdout/stderr of the replayed process, `captured_output_store` can be provided. + +```py +output_store = dict() +# pass dictionary by reference, it will be filled with standard outputs - even if process replay fails +output_logs = replay_process_with_name(['radard', 'plannerd'], lr, captured_output_store=output_store) + +# entries with captured output in format { 'out': '...', 'err': '...' } will be added to provided dictionary for each replayed process +print(output_store['radard']['out']) # radard stdout +print(output_store['radard']['err']) # radard stderr +``` diff --git a/selfdrive/test/process_replay/__init__.py b/selfdrive/test/process_replay/__init__.py new file mode 100644 index 0000000..b994277 --- /dev/null +++ b/selfdrive/test/process_replay/__init__.py @@ -0,0 +1,2 @@ +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, get_process_config, get_custom_params_from_lr, \ + replay_process, replay_process_with_name # noqa: F401 diff --git a/selfdrive/test/process_replay/capture.py b/selfdrive/test/process_replay/capture.py new file mode 100644 index 0000000..90c279e --- /dev/null +++ b/selfdrive/test/process_replay/capture.py @@ -0,0 +1,59 @@ +import os +import sys + +from typing import no_type_check + +class FdRedirect: + def __init__(self, file_prefix: str, fd: int): + fname = os.path.join("/tmp", f"{file_prefix}.{fd}") + if os.path.exists(fname): + os.unlink(fname) + self.dest_fd = os.open(fname, os.O_WRONLY | os.O_CREAT) + self.dest_fname = fname + self.source_fd = fd + os.set_inheritable(self.dest_fd, True) + + def __del__(self): + os.close(self.dest_fd) + + def purge(self) -> None: + os.unlink(self.dest_fname) + + def read(self) -> bytes: + with open(self.dest_fname, "rb") as f: + return f.read() or b"" + + def link(self) -> None: + os.dup2(self.dest_fd, self.source_fd) + + +class ProcessOutputCapture: + def __init__(self, proc_name: str, prefix: str): + prefix = f"{proc_name}_{prefix}" + self.stdout_redirect = FdRedirect(prefix, 1) + self.stderr_redirect = FdRedirect(prefix, 2) + + def __del__(self): + self.stdout_redirect.purge() + self.stderr_redirect.purge() + + @no_type_check # ipython classes have incompatible signatures + def link_with_current_proc(self) -> None: + try: + # prevent ipykernel from redirecting stdout/stderr of python subprocesses + from ipykernel.iostream import OutStream + if isinstance(sys.stdout, OutStream): + sys.stdout = sys.__stdout__ + if isinstance(sys.stderr, OutStream): + sys.stderr = sys.__stderr__ + except ImportError: + pass + + # link stdout/stderr to the fifo + self.stdout_redirect.link() + self.stderr_redirect.link() + + def read_outerr(self) -> tuple[str, str]: + out_str = self.stdout_redirect.read().decode() + err_str = self.stderr_redirect.read().decode() + return out_str, err_str diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py new file mode 100644 index 0000000..673f3b4 --- /dev/null +++ b/selfdrive/test/process_replay/compare_logs.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +import sys +import math +import capnp +import numbers +import dictdiffer +from collections import Counter + +from openpilot.tools.lib.logreader import LogReader + +EPSILON = sys.float_info.epsilon + + +def remove_ignored_fields(msg, ignore): + msg = msg.as_builder() + for key in ignore: + attr = msg + keys = key.split(".") + if msg.which() != keys[0] and len(keys) > 1: + continue + + for k in keys[:-1]: + # indexing into list + if k.isdigit(): + attr = attr[int(k)] + else: + attr = getattr(attr, k) + + v = getattr(attr, keys[-1]) + if isinstance(v, bool): + val = False + elif isinstance(v, numbers.Number): + val = 0 + elif isinstance(v, (list, capnp.lib.capnp._DynamicListBuilder)): + val = [] + else: + raise NotImplementedError(f"Unknown type: {type(v)}") + setattr(attr, keys[-1], val) + return msg + + +def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None,): + if ignore_fields is None: + ignore_fields = [] + if ignore_msgs is None: + ignore_msgs = [] + tolerance = EPSILON if tolerance is None else tolerance + + log1, log2 = ( + [m for m in log if m.which() not in ignore_msgs] + for log in (log1, log2) + ) + + if len(log1) != len(log2): + cnt1 = Counter(m.which() for m in log1) + cnt2 = Counter(m.which() for m in log2) + raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}\n\t\t{cnt1}\n\t\t{cnt2}") + + diff = [] + for msg1, msg2 in zip(log1, log2, strict=True): + if msg1.which() != msg2.which(): + raise Exception("msgs not aligned between logs") + + msg1 = remove_ignored_fields(msg1, ignore_fields) + msg2 = remove_ignored_fields(msg2, ignore_fields) + + if msg1.to_bytes() != msg2.to_bytes(): + msg1_dict = msg1.as_reader().to_dict(verbose=True) + msg2_dict = msg2.as_reader().to_dict(verbose=True) + + dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields) + + # Dictdiffer only supports relative tolerance, we also want to check for absolute + # TODO: add this to dictdiffer + def outside_tolerance(diff): + try: + if diff[0] == "change": + a, b = diff[2] + finite = math.isfinite(a) and math.isfinite(b) + if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number): + return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + except TypeError: + pass + return True + + dd = list(filter(outside_tolerance, dd)) + + diff.extend(dd) + return diff + + +def format_process_diff(diff): + diff_short, diff_long = "", "" + + if isinstance(diff, str): + diff_short += f" {diff}\n" + diff_long += f"\t{diff}\n" + else: + cnt: dict[str, int] = {} + for d in diff: + diff_long += f"\t{str(d)}\n" + + k = str(d[1]) + cnt[k] = 1 if k not in cnt else cnt[k] + 1 + + for k, v in sorted(cnt.items()): + diff_short += f" {k}: {v}\n" + + return diff_short, diff_long + + +def format_diff(results, log_paths, ref_commit): + diff_short, diff_long = "", "" + diff_long += f"***** tested against commit {ref_commit} *****\n" + + failed = False + for segment, result in list(results.items()): + diff_short += f"***** results for segment {segment} *****\n" + diff_long += f"***** differences for segment {segment} *****\n" + + for proc, diff in list(result.items()): + diff_long += f"*** process: {proc} ***\n" + diff_long += f"\tref: {log_paths[segment][proc]['ref']}\n" + diff_long += f"\tnew: {log_paths[segment][proc]['new']}\n\n" + + diff_short += f" {proc}\n" + + if isinstance(diff, str) or len(diff): + diff_short += f" ref: {log_paths[segment][proc]['ref']}\n" + diff_short += f" new: {log_paths[segment][proc]['new']}\n\n" + failed = True + + proc_diff_short, proc_diff_long = format_process_diff(diff) + + diff_long += proc_diff_long + diff_short += proc_diff_short + + return diff_short, diff_long, failed + + +if __name__ == "__main__": + log1 = list(LogReader(sys.argv[1])) + log2 = list(LogReader(sys.argv[2])) + ignore_fields = sys.argv[3:] or ["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"] + results = {"segment": {"proc": compare_logs(log1, log2, ignore_fields)}} + log_paths = {"segment": {"proc": {"ref": sys.argv[1], "new": sys.argv[2]}}} + diff_short, diff_long, failed = format_diff(results, log_paths, None) + + print(diff_long) + print(diff_short) diff --git a/selfdrive/test/process_replay/debayer_replay_ref_commit b/selfdrive/test/process_replay/debayer_replay_ref_commit new file mode 100644 index 0000000..551fc68 --- /dev/null +++ b/selfdrive/test/process_replay/debayer_replay_ref_commit @@ -0,0 +1 @@ +8f9ba7540b4549b4a57312129b8ff678d045f70f \ No newline at end of file diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py new file mode 100644 index 0000000..afcf705 --- /dev/null +++ b/selfdrive/test/process_replay/migration.py @@ -0,0 +1,236 @@ +from collections import defaultdict + +from cereal import messaging +from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index +from openpilot.selfdrive.car.toyota.values import EPS_SCALE +from openpilot.selfdrive.manager.process_config import managed_processes +from panda import Panda + + +# TODO: message migration should happen in-place +def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, camera_states=False): + msgs = migrate_sensorEvents(lr, old_logtime) + msgs = migrate_carParams(msgs, old_logtime) + msgs = migrate_gpsLocation(msgs) + msgs = migrate_deviceState(msgs) + if manager_states: + msgs = migrate_managerState(msgs) + if panda_states: + msgs = migrate_pandaStates(msgs) + msgs = migrate_peripheralState(msgs) + if camera_states: + msgs = migrate_cameraStates(msgs) + + return msgs + + +def migrate_managerState(lr): + all_msgs = [] + for msg in lr: + if msg.which() != "managerState": + all_msgs.append(msg) + continue + + new_msg = msg.as_builder() + new_msg.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] + all_msgs.append(new_msg.as_reader()) + + return all_msgs + + +def migrate_gpsLocation(lr): + all_msgs = [] + for msg in lr: + if msg.which() in ('gpsLocation', 'gpsLocationExternal'): + new_msg = msg.as_builder() + g = getattr(new_msg, new_msg.which()) + # hasFix is a newer field + if not g.hasFix and g.flags == 1: + g.hasFix = True + all_msgs.append(new_msg.as_reader()) + else: + all_msgs.append(msg) + return all_msgs + + +def migrate_deviceState(lr): + all_msgs = [] + dt = None + for msg in lr: + if msg.which() == 'initData': + dt = msg.initData.deviceType + if msg.which() == 'deviceState': + n = msg.as_builder() + n.deviceState.deviceType = dt + all_msgs.append(n.as_reader()) + else: + all_msgs.append(msg) + return all_msgs + + +def migrate_pandaStates(lr): + all_msgs = [] + # TODO: safety param migration should be handled automatically + safety_param_migration = { + "TOYOTA PRIUS 2017": EPS_SCALE["TOYOTA PRIUS 2017"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL, + "TOYOTA RAV4 2017": EPS_SCALE["TOYOTA RAV4 2017"] | Panda.FLAG_TOYOTA_ALT_BRAKE | Panda.FLAG_TOYOTA_GAS_INTERCEPTOR, + "KIA EV6 2022": Panda.FLAG_HYUNDAI_EV_GAS | Panda.FLAG_HYUNDAI_CANFD_HDA2, + } + + # Migrate safety param base on carState + CP = next((m.carParams for m in lr if m.which() == 'carParams'), None) + assert CP is not None, "carParams message not found" + if CP.carFingerprint in safety_param_migration: + safety_param = safety_param_migration[CP.carFingerprint] + elif len(CP.safetyConfigs): + safety_param = CP.safetyConfigs[0].safetyParam + if CP.safetyConfigs[0].safetyParamDEPRECATED != 0: + safety_param = CP.safetyConfigs[0].safetyParamDEPRECATED + else: + safety_param = CP.safetyParamDEPRECATED + + for msg in lr: + if msg.which() == 'pandaStateDEPRECATED': + new_msg = messaging.new_message('pandaStates', 1) + new_msg.valid = msg.valid + new_msg.logMonoTime = msg.logMonoTime + new_msg.pandaStates[0] = msg.pandaStateDEPRECATED + new_msg.pandaStates[0].safetyParam = safety_param + all_msgs.append(new_msg.as_reader()) + elif msg.which() == 'pandaStates': + new_msg = msg.as_builder() + new_msg.pandaStates[-1].safetyParam = safety_param + all_msgs.append(new_msg.as_reader()) + else: + all_msgs.append(msg) + + return all_msgs + + +def migrate_peripheralState(lr): + if any(msg.which() == "peripheralState" for msg in lr): + return lr + + all_msg = [] + for msg in lr: + all_msg.append(msg) + if msg.which() not in ["pandaStates", "pandaStateDEPRECATED"]: + continue + + new_msg = messaging.new_message("peripheralState") + new_msg.valid = msg.valid + new_msg.logMonoTime = msg.logMonoTime + all_msg.append(new_msg.as_reader()) + + return all_msg + + +def migrate_cameraStates(lr): + all_msgs = [] + frame_to_encode_id = defaultdict(dict) + # just for encodeId fallback mechanism + min_frame_id = defaultdict(lambda: float('inf')) + + for msg in lr: + if msg.which() not in ["roadEncodeIdx", "wideRoadEncodeIdx", "driverEncodeIdx"]: + continue + + encode_index = getattr(msg, msg.which()) + meta = meta_from_encode_index(msg.which()) + + assert encode_index.segmentId < 1200, f"Encoder index segmentId greater that 1200: {msg.which()} {encode_index.segmentId}" + frame_to_encode_id[meta.camera_state][encode_index.frameId] = encode_index.segmentId + + for msg in lr: + if msg.which() not in ["roadCameraState", "wideRoadCameraState", "driverCameraState"]: + all_msgs.append(msg) + continue + + camera_state = getattr(msg, msg.which()) + min_frame_id[msg.which()] = min(min_frame_id[msg.which()], camera_state.frameId) + + encode_id = frame_to_encode_id[msg.which()].get(camera_state.frameId) + if encode_id is None: + print(f"Missing encoded frame for camera feed {msg.which()} with frameId: {camera_state.frameId}") + if len(frame_to_encode_id[msg.which()]) != 0: + continue + + # fallback mechanism for logs without encodeIdx (e.g. logs from before 2022 with dcamera recording disabled) + # try to fake encode_id by subtracting lowest frameId + encode_id = camera_state.frameId - min_frame_id[msg.which()] + print(f"Faking encodeId to {encode_id} for camera feed {msg.which()} with frameId: {camera_state.frameId}") + + new_msg = messaging.new_message(msg.which()) + new_camera_state = getattr(new_msg, new_msg.which()) + new_camera_state.frameId = encode_id + new_camera_state.encodeId = encode_id + # timestampSof was added later so it might be missing on some old segments + if camera_state.timestampSof == 0 and camera_state.timestampEof > 25000000: + new_camera_state.timestampSof = camera_state.timestampEof - 18000000 + else: + new_camera_state.timestampSof = camera_state.timestampSof + new_camera_state.timestampEof = camera_state.timestampEof + new_msg.logMonoTime = msg.logMonoTime + new_msg.valid = msg.valid + + all_msgs.append(new_msg.as_reader()) + + return all_msgs + + +def migrate_carParams(lr, old_logtime=False): + all_msgs = [] + for msg in lr: + if msg.which() == 'carParams': + CP = messaging.new_message('carParams') + CP.valid = True + CP.carParams = msg.carParams.as_builder() + for car_fw in CP.carParams.carFw: + car_fw.brand = CP.carParams.carName + if old_logtime: + CP.logMonoTime = msg.logMonoTime + msg = CP.as_reader() + all_msgs.append(msg) + + return all_msgs + + +def migrate_sensorEvents(lr, old_logtime=False): + all_msgs = [] + for msg in lr: + if msg.which() != 'sensorEventsDEPRECATED': + all_msgs.append(msg) + continue + + # migrate to split sensor events + for evt in msg.sensorEventsDEPRECATED: + # build new message for each sensor type + sensor_service = '' + if evt.which() == 'acceleration': + sensor_service = 'accelerometer' + elif evt.which() == 'gyro' or evt.which() == 'gyroUncalibrated': + sensor_service = 'gyroscope' + elif evt.which() == 'light' or evt.which() == 'proximity': + sensor_service = 'lightSensor' + elif evt.which() == 'magnetic' or evt.which() == 'magneticUncalibrated': + sensor_service = 'magnetometer' + elif evt.which() == 'temperature': + sensor_service = 'temperatureSensor' + + m = messaging.new_message(sensor_service) + m.valid = True + if old_logtime: + m.logMonoTime = msg.logMonoTime + + m_dat = getattr(m, sensor_service) + m_dat.version = evt.version + m_dat.sensor = evt.sensor + m_dat.type = evt.type + m_dat.source = evt.source + if old_logtime: + m_dat.timestamp = evt.timestamp + setattr(m_dat, evt.which(), getattr(evt, evt.which())) + + all_msgs.append(m.as_reader()) + + return all_msgs diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py new file mode 100644 index 0000000..9489528 --- /dev/null +++ b/selfdrive/test/process_replay/model_replay.py @@ -0,0 +1,253 @@ +#!/usr/bin/env python3 +import os +import sys +import time +from collections import defaultdict +from typing import Any + +import cereal.messaging as messaging +from openpilot.common.params import Params +from openpilot.system.hardware import PC +from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.tools.lib.openpilotci import BASE_URL, get_url +from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff +from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process +from openpilot.system.version import get_commit +from openpilot.tools.lib.framereader import FrameReader +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.helpers import save_log + +TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30" +SEGMENT = 6 +MAX_FRAMES = 100 if PC else 600 +NAV_FRAMES = 50 + +NO_NAV = "NO_NAV" in os.environ +NO_MODEL = "NO_MODEL" in os.environ +SEND_EXTRA_INPUTS = bool(int(os.getenv("SEND_EXTRA_INPUTS", "0"))) + + +def get_log_fn(ref_commit, test_route): + return f"{test_route}_model_tici_{ref_commit}.bz2" + + +def trim_logs_to_max_frames(logs, max_frames, frs_types, include_all_types): + all_msgs = [] + cam_state_counts = defaultdict(int) + # keep adding messages until cam states are equal to MAX_FRAMES + for msg in sorted(logs, key=lambda m: m.logMonoTime): + all_msgs.append(msg) + if msg.which() in frs_types: + cam_state_counts[msg.which()] += 1 + + if all(cam_state_counts[state] == max_frames for state in frs_types): + break + + if len(include_all_types) != 0: + other_msgs = [m for m in logs if m.which() in include_all_types] + all_msgs.extend(other_msgs) + + return all_msgs + + +def nav_model_replay(lr): + sm = messaging.SubMaster(['navModel', 'navThumbnail', 'mapRenderState']) + pm = messaging.PubMaster(['liveLocationKalman', 'navRoute']) + + nav = [m for m in lr if m.which() == 'navRoute'] + llk = [m for m in lr if m.which() == 'liveLocationKalman'] + assert len(nav) > 0 and len(llk) >= NAV_FRAMES and nav[0].logMonoTime < llk[-NAV_FRAMES].logMonoTime + + log_msgs = [] + try: + assert "MAPBOX_TOKEN" in os.environ + os.environ['MAP_RENDER_TEST_MODE'] = '1' + Params().put_bool('DmModelInitialized', True) + managed_processes['mapsd'].start() + managed_processes['navmodeld'].start() + + # setup position and route + for _ in range(10): + for s in (llk[-NAV_FRAMES], nav[0]): + pm.send(s.which(), s.as_builder().to_bytes()) + sm.update(1000) + if sm.updated['navModel']: + break + time.sleep(1) + + if not sm.updated['navModel']: + raise Exception("no navmodeld outputs, failed to initialize") + + # drain + time.sleep(2) + sm.update(0) + + # run replay + for n in range(len(llk) - NAV_FRAMES, len(llk)): + pm.send(llk[n].which(), llk[n].as_builder().to_bytes()) + m = messaging.recv_one(sm.sock['navThumbnail']) + assert m is not None, f"no navThumbnail, frame={n}" + log_msgs.append(m) + + m = messaging.recv_one(sm.sock['mapRenderState']) + assert m is not None, f"no mapRenderState, frame={n}" + log_msgs.append(m) + + m = messaging.recv_one(sm.sock['navModel']) + assert m is not None, f"no navModel response, frame={n}" + log_msgs.append(m) + finally: + managed_processes['mapsd'].stop() + managed_processes['navmodeld'].stop() + + return log_msgs + + +def model_replay(lr, frs): + # modeld is using frame pairs + modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}) + dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"}) + + if not SEND_EXTRA_INPUTS: + modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration'] + dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'liveCalibration'] + + # initial setup + for s in ('liveCalibration', 'deviceState'): + msg = next(msg for msg in lr if msg.which() == s).as_builder() + msg.logMonoTime = lr[0].logMonoTime + modeld_logs.insert(1, msg.as_reader()) + dmodeld_logs.insert(1, msg.as_reader()) + + modeld = get_process_config("modeld") + dmonitoringmodeld = get_process_config("dmonitoringmodeld") + + modeld_msgs = replay_process(modeld, modeld_logs, frs) + dmonitoringmodeld_msgs = replay_process(dmonitoringmodeld, dmodeld_logs, frs) + return modeld_msgs + dmonitoringmodeld_msgs + + +if __name__ == "__main__": + update = "--update" in sys.argv + replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit") + + # load logs + lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT))) + frs = { + 'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera"), readahead=True), + 'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera"), readahead=True), + 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"), readahead=True) + } + + # Update tile refs + if update: + import urllib + import requests + import threading + import http.server + from openpilot.tools.lib.openpilotci import upload_bytes + os.environ['MAPS_HOST'] = 'http://localhost:5000' + + class HTTPRequestHandler(http.server.BaseHTTPRequestHandler): + def do_GET(self): + assert len(self.path) > 10 # Sanity check on path length + r = requests.get(f'https://api.mapbox.com{self.path}', timeout=30) + upload_bytes(r.content, urllib.parse.urlparse(self.path).path.lstrip('/')) + self.send_response(r.status_code) + self.send_header('Content-type','text/html') + self.end_headers() + self.wfile.write(r.content) + + server = http.server.HTTPServer(("127.0.0.1", 5000), HTTPRequestHandler) + thread = threading.Thread(None, server.serve_forever, daemon=True) + thread.start() + else: + os.environ['MAPS_HOST'] = BASE_URL.rstrip('/') + + log_msgs = [] + # run replays + if not NO_MODEL: + log_msgs += model_replay(lr, frs) + if not NO_NAV: + log_msgs += nav_model_replay(lr) + + # get diff + failed = False + if not update: + with open(ref_commit_fn) as f: + ref_commit = f.read().strip() + log_fn = get_log_fn(ref_commit, TEST_ROUTE) + try: + all_logs = list(LogReader(BASE_URL + log_fn)) + cmp_log = [] + + # logs are ordered based on type: modelV2, driverStateV2, nav messages (navThumbnail, mapRenderState, navModel) + if not NO_MODEL: + model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "cameraOdometry")) + cmp_log += all_logs[model_start_index:model_start_index + MAX_FRAMES*2] + dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2") + cmp_log += all_logs[dmon_start_index:dmon_start_index + MAX_FRAMES] + if not NO_NAV: + nav_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ["navThumbnail", "mapRenderState", "navModel"]) + nav_logs = all_logs[nav_start_index:nav_start_index + NAV_FRAMES*3] + cmp_log += nav_logs + + ignore = [ + 'logMonoTime', + 'modelV2.frameDropPerc', + 'modelV2.modelExecutionTime', + 'driverStateV2.modelExecutionTime', + 'driverStateV2.dspExecutionTime', + 'navModel.dspExecutionTime', + 'navModel.modelExecutionTime', + 'navThumbnail.timestampEof', + 'mapRenderState.locationMonoTime', + 'mapRenderState.renderTime', + ] + if PC: + ignore += [ + 'modelV2.laneLines.0.t', + 'modelV2.laneLines.1.t', + 'modelV2.laneLines.2.t', + 'modelV2.laneLines.3.t', + 'modelV2.roadEdges.0.t', + 'modelV2.roadEdges.1.t', + ] + # TODO this tolerance is absurdly large + tolerance = 2.0 if PC else None + results: Any = {TEST_ROUTE: {}} + log_paths: Any = {TEST_ROUTE: {"models": {'ref': BASE_URL + log_fn, 'new': log_fn}}} + results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) + diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit) + + if "CI" in os.environ: + print(diff_long) + print('-------------\n'*5) + print(diff_short) + with open("model_diff.txt", "w") as f: + f.write(diff_long) + except Exception as e: + print(str(e)) + failed = True + + # upload new refs + if (update or failed) and not PC: + from openpilot.tools.lib.openpilotci import upload_file + + print("Uploading new refs") + + new_commit = get_commit() + log_fn = get_log_fn(new_commit, TEST_ROUTE) + save_log(log_fn, log_msgs) + try: + upload_file(log_fn, os.path.basename(log_fn)) + except Exception as e: + print("failed to upload", e) + + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) + + print("\n\nNew ref commit: ", new_commit) + + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit new file mode 100644 index 0000000..85ba5fb --- /dev/null +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -0,0 +1 @@ +60b00d102b3aedcc74a91722d1210cc6905b0c8f diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py new file mode 100644 index 0000000..233f5d7 --- /dev/null +++ b/selfdrive/test/process_replay/process_replay.py @@ -0,0 +1,800 @@ +#!/usr/bin/env python3 +import os +import time +import copy +import json +import heapq +import signal +import platform +from collections import OrderedDict +from dataclasses import dataclass, field +from typing import Any +from collections.abc import Callable, Iterable +from tqdm import tqdm +import capnp + +import cereal.messaging as messaging +from cereal import car +from cereal.services import SERVICE_LIST +from cereal.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.common.timeout import Timeout +from openpilot.common.realtime import DT_CTRL +from panda.python import ALTERNATIVE_EXPERIENCE +from openpilot.selfdrive.car.car_helpers import get_car, interfaces +from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams +from openpilot.selfdrive.test.process_replay.migration import migrate_all +from openpilot.selfdrive.test.process_replay.capture import ProcessOutputCapture +from openpilot.tools.lib.logreader import LogIterable +from openpilot.tools.lib.framereader import BaseFrameReader + +# Numpy gives different results based on CPU features after version 19 +NUMPY_TOLERANCE = 1e-7 +PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__)) +FAKEDATA = os.path.join(PROC_REPLAY_DIR, "fakedata/") + +class DummySocket: + def __init__(self): + self.data: list[bytes] = [] + + def receive(self, non_blocking: bool = False) -> bytes | None: + if non_blocking: + return None + + return self.data.pop() + + def send(self, data: bytes): + self.data.append(data) + +class LauncherWithCapture: + def __init__(self, capture: ProcessOutputCapture, launcher: Callable): + self.capture = capture + self.launcher = launcher + + def __call__(self, *args, **kwargs): + self.capture.link_with_current_proc() + self.launcher(*args, **kwargs) + + +class ReplayContext: + def __init__(self, cfg): + self.proc_name = cfg.proc_name + self.pubs = cfg.pubs + self.main_pub = cfg.main_pub + self.main_pub_drained = cfg.main_pub_drained + self.unlocked_pubs = cfg.unlocked_pubs + assert(len(self.pubs) != 0 or self.main_pub is not None) + + def __enter__(self): + self.open_context() + + return self + + def __exit__(self, exc_type, exc_obj, exc_tb): + self.close_context() + + def open_context(self): + messaging.toggle_fake_events(True) + messaging.set_fake_prefix(self.proc_name) + + if self.main_pub is None: + self.events = OrderedDict() + pubs_with_events = [pub for pub in self.pubs if pub not in self.unlocked_pubs] + for pub in pubs_with_events: + self.events[pub] = messaging.fake_event_handle(pub, enable=True) + else: + self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)} + + def close_context(self): + del self.events + + messaging.toggle_fake_events(False) + messaging.delete_fake_prefix() + + @property + def all_recv_called_events(self): + return [man.recv_called_event for man in self.events.values()] + + @property + def all_recv_ready_events(self): + return [man.recv_ready_event for man in self.events.values()] + + def send_sync(self, pm, endpoint, dat): + self.events[endpoint].recv_called_event.wait() + self.events[endpoint].recv_called_event.clear() + pm.send(endpoint, dat) + self.events[endpoint].recv_ready_event.set() + + def unlock_sockets(self): + expected_sets = len(self.events) + while expected_sets > 0: + index = messaging.wait_for_one_event(self.all_recv_called_events) + self.all_recv_called_events[index].clear() + self.all_recv_ready_events[index].set() + expected_sets -= 1 + + def wait_for_recv_called(self): + messaging.wait_for_one_event(self.all_recv_called_events) + + def wait_for_next_recv(self, trigger_empty_recv): + index = messaging.wait_for_one_event(self.all_recv_called_events) + if self.main_pub is not None and self.main_pub_drained and trigger_empty_recv: + self.all_recv_called_events[index].clear() + self.all_recv_ready_events[index].set() + self.all_recv_called_events[index].wait() + + +@dataclass +class ProcessConfig: + proc_name: str + pubs: list[str] + subs: list[str] + ignore: list[str] + config_callback: Callable | None = None + init_callback: Callable | None = None + should_recv_callback: Callable | None = None + tolerance: float | None = None + processing_time: float = 0.001 + timeout: int = 30 + simulation: bool = True + main_pub: str | None = None + main_pub_drained: bool = True + vision_pubs: list[str] = field(default_factory=list) + ignore_alive_pubs: list[str] = field(default_factory=list) + unlocked_pubs: list[str] = field(default_factory=list) + + +class ProcessContainer: + def __init__(self, cfg: ProcessConfig): + self.prefix = OpenpilotPrefix(clean_dirs_on_exit=False) + self.cfg = copy.deepcopy(cfg) + self.process = copy.deepcopy(managed_processes[cfg.proc_name]) + self.msg_queue: list[capnp._DynamicStructReader] = [] + self.cnt = 0 + self.pm: messaging.PubMaster | None = None + self.sockets: list[messaging.SubSocket] | None = None + self.rc: ReplayContext | None = None + self.vipc_server: VisionIpcServer | None = None + self.environ_config: dict[str, Any] | None = None + self.capture: ProcessOutputCapture | None = None + + @property + def has_empty_queue(self) -> bool: + return len(self.msg_queue) == 0 + + @property + def pubs(self) -> list[str]: + return self.cfg.pubs + + @property + def subs(self) -> list[str]: + return self.cfg.subs + + def _clean_env(self): + for k in self.environ_config.keys(): + if k in os.environ: + del os.environ[k] + + for k in ["PROC_NAME", "SIMULATION"]: + if k in os.environ: + del os.environ[k] + + def _setup_env(self, params_config: dict[str, Any], environ_config: dict[str, Any]): + for k, v in environ_config.items(): + if len(v) != 0: + os.environ[k] = v + elif k in os.environ: + del os.environ[k] + + os.environ["PROC_NAME"] = self.cfg.proc_name + if self.cfg.simulation: + os.environ["SIMULATION"] = "1" + elif "SIMULATION" in os.environ: + del os.environ["SIMULATION"] + + params = Params() + for k, v in params_config.items(): + if isinstance(v, bool): + params.put_bool(k, v) + else: + params.put(k, v) + + self.environ_config = environ_config + + def _setup_vision_ipc(self, all_msgs: LogIterable, frs: dict[str, Any]): + assert len(self.cfg.vision_pubs) != 0 + + vipc_server = VisionIpcServer("camerad") + streams_metas = available_streams(all_msgs) + for meta in streams_metas: + if meta.camera_state in self.cfg.vision_pubs: + frame_size = (frs[meta.camera_state].w, frs[meta.camera_state].h) + vipc_server.create_buffers(meta.stream, 2, False, *frame_size) + vipc_server.start_listener() + + self.vipc_server = vipc_server + self.cfg.vision_pubs = [meta.camera_state for meta in streams_metas if meta.camera_state in self.cfg.vision_pubs] + + def _start_process(self): + if self.capture is not None: + self.process.launcher = LauncherWithCapture(self.capture, self.process.launcher) + self.process.prepare() + self.process.start() + + def start( + self, params_config: dict[str, Any], environ_config: dict[str, Any], + all_msgs: LogIterable, frs: dict[str, BaseFrameReader] | None, + fingerprint: str | None, capture_output: bool + ): + with self.prefix as p: + self._setup_env(params_config, environ_config) + + if self.cfg.config_callback is not None: + params = Params() + self.cfg.config_callback(params, self.cfg, all_msgs) + + self.rc = ReplayContext(self.cfg) + self.rc.open_context() + + self.pm = messaging.PubMaster(self.cfg.pubs) + self.sockets = [messaging.sub_sock(s, timeout=100) for s in self.cfg.subs] + + if len(self.cfg.vision_pubs) != 0: + assert frs is not None + self._setup_vision_ipc(all_msgs, frs) + assert self.vipc_server is not None + + if capture_output: + self.capture = ProcessOutputCapture(self.cfg.proc_name, p.prefix) + + self._start_process() + + if self.cfg.init_callback is not None: + self.cfg.init_callback(self.rc, self.pm, all_msgs, fingerprint) + + # wait for process to startup + with Timeout(10, error_msg=f"timed out waiting for process to start: {repr(self.cfg.proc_name)}"): + while not all(self.pm.all_readers_updated(s) for s in self.cfg.pubs if s not in self.cfg.ignore_alive_pubs): + time.sleep(0) + + def stop(self): + with self.prefix: + self.process.signal(signal.SIGKILL) + self.process.stop() + self.rc.close_context() + self.prefix.clean_dirs() + self._clean_env() + + def run_step(self, msg: capnp._DynamicStructReader, frs: dict[str, BaseFrameReader] | None) -> list[capnp._DynamicStructReader]: + assert self.rc and self.pm and self.sockets and self.process.proc + + output_msgs = [] + with self.prefix, Timeout(self.cfg.timeout, error_msg=f"timed out testing process {repr(self.cfg.proc_name)}"): + end_of_cycle = True + if self.cfg.should_recv_callback is not None: + end_of_cycle = self.cfg.should_recv_callback(msg, self.cfg, self.cnt) + + self.msg_queue.append(msg) + if end_of_cycle: + self.rc.wait_for_recv_called() + + # call recv to let sub-sockets reconnect, after we know the process is ready + if self.cnt == 0: + for s in self.sockets: + messaging.recv_one_or_none(s) + + # empty recv on drained pub indicates the end of messages, only do that if there're any + trigger_empty_recv = False + if self.cfg.main_pub and self.cfg.main_pub_drained: + trigger_empty_recv = next((True for m in self.msg_queue if m.which() == self.cfg.main_pub), False) + + for m in self.msg_queue: + self.pm.send(m.which(), m.as_builder()) + # send frames if needed + if self.vipc_server is not None and m.which() in self.cfg.vision_pubs: + camera_state = getattr(m, m.which()) + camera_meta = meta_from_camera_state(m.which()) + assert frs is not None + img = frs[m.which()].get(camera_state.frameId, pix_fmt="nv12")[0] + self.vipc_server.send(camera_meta.stream, img.flatten().tobytes(), + camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) + self.msg_queue = [] + + self.rc.unlock_sockets() + self.rc.wait_for_next_recv(trigger_empty_recv) + + for socket in self.sockets: + ms = messaging.drain_sock(socket) + for m in ms: + m = m.as_builder() + m.logMonoTime = msg.logMonoTime + int(self.cfg.processing_time * 1e9) + output_msgs.append(m.as_reader()) + self.cnt += 1 + assert self.process.proc.is_alive() + + return output_msgs + + +def controlsd_fingerprint_callback(rc, pm, msgs, fingerprint): + print("start fingerprinting") + params = Params() + canmsgs = [msg for msg in msgs if msg.which() == "can"][:300] + + # controlsd expects one arbitrary can and pandaState + rc.send_sync(pm, "can", messaging.new_message("can", 1)) + pm.send("pandaStates", messaging.new_message("pandaStates", 1)) + rc.send_sync(pm, "can", messaging.new_message("can", 1)) + rc.wait_for_next_recv(True) + + # fingerprinting is done, when CarParams is set + while params.get("CarParams") is None: + if len(canmsgs) == 0: + raise ValueError("Fingerprinting failed. Run out of can msgs") + + m = canmsgs.pop(0) + rc.send_sync(pm, "can", m.as_builder().to_bytes()) + rc.wait_for_next_recv(False) + + +def get_car_params_callback(rc, pm, msgs, fingerprint): + params = Params() + if fingerprint: + CarInterface, _, _ = interfaces[fingerprint] + CP = CarInterface.get_non_essential_params(fingerprint) + else: + can = DummySocket() + sendcan = DummySocket() + + canmsgs = [msg for msg in msgs if msg.which() == "can"] + has_cached_cp = params.get("CarParamsCache") is not None + assert len(canmsgs) != 0, "CAN messages are required for fingerprinting" + assert os.environ.get("SKIP_FW_QUERY", False) or has_cached_cp, \ + "CarParamsCache is required for fingerprinting. Make sure to keep carParams msgs in the logs." + + for m in canmsgs[:300]: + can.send(m.as_builder().to_bytes()) + _, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled")) + params.put("CarParams", CP.to_bytes()) + return CP + + +def controlsd_rcv_callback(msg, cfg, frame): + # no sendcan until controlsd is initialized + if msg.which() != "can": + return False + + socks = [ + s for s in cfg.subs if + frame % int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency) == 0 + ] + if "sendcan" in socks and (frame - 1) < 2000: + socks.remove("sendcan") + return len(socks) > 0 + + +def calibration_rcv_callback(msg, cfg, frame): + # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets. + # should_recv always true to increment frame + return (frame - 1) == 0 or msg.which() == 'cameraOdometry' + + +def torqued_rcv_callback(msg, cfg, frame): + # should_recv always true to increment frame + return (frame - 1) == 0 or msg.which() == 'liveLocationKalman' + + +def dmonitoringmodeld_rcv_callback(msg, cfg, frame): + return msg.which() == "driverCameraState" + + +class ModeldCameraSyncRcvCallback: + def __init__(self): + self.road_present = False + self.wide_road_present = False + self.is_dual_camera = True + + def __call__(self, msg, cfg, frame): + self.is_dual_camera = len(cfg.vision_pubs) == 2 + if msg.which() == "roadCameraState": + self.road_present = True + elif msg.which() == "wideRoadCameraState": + self.wide_road_present = True + + if self.road_present and self.wide_road_present: + self.road_present, self.wide_road_present = False, False + return True + elif self.road_present and not self.is_dual_camera: + self.road_present = False + return True + else: + return False + + +class MessageBasedRcvCallback: + def __init__(self, trigger_msg_type): + self.trigger_msg_type = trigger_msg_type + + def __call__(self, msg, cfg, frame): + return msg.which() == self.trigger_msg_type + + +class FrequencyBasedRcvCallback: + def __init__(self, trigger_msg_type): + self.trigger_msg_type = trigger_msg_type + + def __call__(self, msg, cfg, frame): + if msg.which() != self.trigger_msg_type: + return False + + resp_sockets = [ + s for s in cfg.subs + if frame % max(1, int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency)) == 0 + ] + return bool(len(resp_sockets)) + + +def controlsd_config_callback(params, cfg, lr): + controlsState = None + initialized = False + for msg in lr: + if msg.which() == "controlsState": + controlsState = msg.controlsState + if initialized: + break + elif msg.which() == "onroadEvents": + initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.onroadEvents] + + assert controlsState is not None and initialized, "controlsState never initialized" + params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) + + +def locationd_config_pubsub_callback(params, cfg, lr): + ublox = params.get_bool("UbloxAvailable") + sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) + + cfg.pubs = set(cfg.pubs) - sub_keys + + +CONFIGS = [ + ProcessConfig( + proc_name="controlsd", + pubs=[ + "can", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", + "longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState", + "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", + "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope" + ], + subs=["controlsState", "carState", "carControl", "sendcan", "onroadEvents", "carParams"], + ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"], + config_callback=controlsd_config_callback, + init_callback=controlsd_fingerprint_callback, + should_recv_callback=controlsd_rcv_callback, + tolerance=NUMPY_TOLERANCE, + processing_time=0.004, + main_pub="can", + ), + ProcessConfig( + proc_name="radard", + pubs=["can", "carState", "modelV2"], + subs=["radarState", "liveTracks"], + ignore=["logMonoTime", "radarState.cumLagMs"], + init_callback=get_car_params_callback, + should_recv_callback=MessageBasedRcvCallback("can"), + main_pub="can", + ), + ProcessConfig( + proc_name="plannerd", + pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"], + subs=["longitudinalPlan", "uiPlan"], + ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], + init_callback=get_car_params_callback, + should_recv_callback=FrequencyBasedRcvCallback("modelV2"), + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="calibrationd", + pubs=["carState", "cameraOdometry", "carParams"], + subs=["liveCalibration"], + ignore=["logMonoTime"], + should_recv_callback=calibration_rcv_callback, + ), + ProcessConfig( + proc_name="dmonitoringd", + pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "controlsState"], + subs=["driverMonitoringState"], + ignore=["logMonoTime"], + should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"), + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="locationd", + pubs=[ + "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", + "liveCalibration", "carState", "gpsLocation" + ], + subs=["liveLocationKalman"], + ignore=["logMonoTime"], + config_callback=locationd_config_pubsub_callback, + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="paramsd", + pubs=["liveLocationKalman", "carState"], + subs=["liveParameters"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=FrequencyBasedRcvCallback("liveLocationKalman"), + tolerance=NUMPY_TOLERANCE, + processing_time=0.004, + ), + ProcessConfig( + proc_name="ubloxd", + pubs=["ubloxRaw"], + subs=["ubloxGnss", "gpsLocationExternal"], + ignore=["logMonoTime"], + ), + ProcessConfig( + proc_name="torqued", + pubs=["liveLocationKalman", "carState", "carControl"], + subs=["liveTorqueParameters"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=torqued_rcv_callback, + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="modeld", + pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"], + subs=["modelV2", "cameraOdometry"], + ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"], + should_recv_callback=ModeldCameraSyncRcvCallback(), + tolerance=NUMPY_TOLERANCE, + processing_time=0.020, + main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("roadCameraState").stream), + main_pub_drained=False, + vision_pubs=["roadCameraState", "wideRoadCameraState"], + ignore_alive_pubs=["wideRoadCameraState"], + init_callback=get_car_params_callback, + ), + ProcessConfig( + proc_name="dmonitoringmodeld", + pubs=["liveCalibration", "driverCameraState"], + subs=["driverStateV2"], + ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.dspExecutionTime"], + should_recv_callback=dmonitoringmodeld_rcv_callback, + tolerance=NUMPY_TOLERANCE, + processing_time=0.020, + main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream), + main_pub_drained=False, + vision_pubs=["driverCameraState"], + ignore_alive_pubs=["driverCameraState"], + ), +] + + +def get_process_config(name: str) -> ProcessConfig: + try: + return copy.deepcopy(next(c for c in CONFIGS if c.proc_name == name)) + except StopIteration as ex: + raise Exception(f"Cannot find process config with name: {name}") from ex + + +def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> dict[str, Any]: + """ + Use this to get custom params dict based on provided logs. + Useful when replaying following processes: calibrationd, paramsd, torqued + The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs. + """ + + car_params = [m for m in lr if m.which() == "carParams"] + live_calibration = [m for m in lr if m.which() == "liveCalibration"] + live_parameters = [m for m in lr if m.which() == "liveParameters"] + live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"] + + assert initial_state in ["first", "last"] + msg_index = 0 if initial_state == "first" else -1 + + assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute" + CP = car_params[msg_index].carParams + + custom_params = { + "CarParamsPrevRoute": CP.as_builder().to_bytes() + } + + if len(live_calibration) > 0: + custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() + if len(live_parameters) > 0: + lp_dict = live_parameters[msg_index].to_dict() + lp_dict["carFingerprint"] = CP.carFingerprint + custom_params["LiveParameters"] = json.dumps(lp_dict) + if len(live_torque_parameters) > 0: + custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() + + return custom_params + + +def replay_process_with_name(name: str | Iterable[str], lr: LogIterable, *args, **kwargs) -> list[capnp._DynamicStructReader]: + if isinstance(name, str): + cfgs = [get_process_config(name)] + elif isinstance(name, Iterable): + cfgs = [get_process_config(n) for n in name] + else: + raise ValueError("name must be str or collections of strings") + + return replay_process(cfgs, lr, *args, **kwargs) + + +def replay_process( + cfg: ProcessConfig | Iterable[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] = None, + fingerprint: str = None, return_all_logs: bool = False, custom_params: dict[str, Any] = None, + captured_output_store: dict[str, dict[str, str]] = None, disable_progress: bool = False +) -> list[capnp._DynamicStructReader]: + if isinstance(cfg, Iterable): + cfgs = list(cfg) + else: + cfgs = [cfg] + + all_msgs = migrate_all(lr, old_logtime=True, + manager_states=True, + panda_states=any("pandaStates" in cfg.pubs for cfg in cfgs), + camera_states=any(len(cfg.vision_pubs) != 0 for cfg in cfgs)) + process_logs = _replay_multi_process(cfgs, all_msgs, frs, fingerprint, custom_params, captured_output_store, disable_progress) + + if return_all_logs: + keys = {m.which() for m in process_logs} + modified_logs = [m for m in all_msgs if m.which() not in keys] + modified_logs.extend(process_logs) + modified_logs.sort(key=lambda m: int(m.logMonoTime)) + log_msgs = modified_logs + else: + log_msgs = process_logs + + return log_msgs + + +def _replay_multi_process( + cfgs: list[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] | None, fingerprint: str | None, + custom_params: dict[str, Any] | None, captured_output_store: dict[str, dict[str, str]] | None, disable_progress: bool +) -> list[capnp._DynamicStructReader]: + if fingerprint is not None: + params_config = generate_params_config(lr=lr, fingerprint=fingerprint, custom_params=custom_params) + env_config = generate_environ_config(fingerprint=fingerprint) + else: + CP = next((m.carParams for m in lr if m.which() == "carParams"), None) + params_config = generate_params_config(lr=lr, CP=CP, custom_params=custom_params) + env_config = generate_environ_config(CP=CP) + + # validate frs and vision pubs + all_vision_pubs = [pub for cfg in cfgs for pub in cfg.vision_pubs] + if len(all_vision_pubs) != 0: + assert frs is not None, "frs must be provided when replaying process using vision streams" + assert all(meta_from_camera_state(st) is not None for st in all_vision_pubs), \ + f"undefined vision stream spotted, probably misconfigured process: (vision pubs: {all_vision_pubs})" + required_vision_pubs = {m.camera_state for m in available_streams(lr)} & set(all_vision_pubs) + assert all(st in frs for st in required_vision_pubs), f"frs for this process must contain following vision streams: {required_vision_pubs}" + + all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) + log_msgs = [] + try: + containers = [] + for cfg in cfgs: + container = ProcessContainer(cfg) + containers.append(container) + container.start(params_config, env_config, all_msgs, frs, fingerprint, captured_output_store is not None) + + all_pubs = {pub for container in containers for pub in container.pubs} + all_subs = {sub for container in containers for sub in container.subs} + lr_pubs = all_pubs - all_subs + pubs_to_containers = {pub: [container for container in containers if pub in container.pubs] for pub in all_pubs} + + pub_msgs = [msg for msg in all_msgs if msg.which() in lr_pubs] + # external queue for messages taken from logs; internal queue for messages generated by processes, which will be republished + external_pub_queue: list[capnp._DynamicStructReader] = pub_msgs.copy() + internal_pub_queue: list[capnp._DynamicStructReader] = [] + # heap for maintaining the order of messages generated by processes, where each element: (logMonoTime, index in internal_pub_queue) + internal_pub_index_heap: list[tuple[int, int]] = [] + + pbar = tqdm(total=len(external_pub_queue), disable=disable_progress) + while len(external_pub_queue) != 0 or (len(internal_pub_index_heap) != 0 and not all(c.has_empty_queue for c in containers)): + if len(internal_pub_index_heap) == 0 or (len(external_pub_queue) != 0 and external_pub_queue[0].logMonoTime < internal_pub_index_heap[0][0]): + msg = external_pub_queue.pop(0) + pbar.update(1) + else: + _, index = heapq.heappop(internal_pub_index_heap) + msg = internal_pub_queue[index] + + target_containers = pubs_to_containers[msg.which()] + for container in target_containers: + output_msgs = container.run_step(msg, frs) + for m in output_msgs: + if m.which() in all_pubs: + internal_pub_queue.append(m) + heapq.heappush(internal_pub_index_heap, (m.logMonoTime, len(internal_pub_queue) - 1)) + log_msgs.extend(output_msgs) + finally: + for container in containers: + container.stop() + if captured_output_store is not None: + assert container.capture is not None + out, err = container.capture.read_outerr() + captured_output_store[container.cfg.proc_name] = {"out": out, "err": err} + + return log_msgs + + +def generate_params_config(lr=None, CP=None, fingerprint=None, custom_params=None) -> dict[str, Any]: + params_dict = { + "OpenpilotEnabledToggle": True, + "DisengageOnAccelerator": True, + "DisableLogging": False, + } + + if custom_params is not None: + params_dict.update(custom_params) + if lr is not None: + has_ublox = any(msg.which() == "ubloxGnss" for msg in lr) + params_dict["UbloxAvailable"] = has_ublox + is_rhd = next((msg.driverMonitoringState.isRHD for msg in lr if msg.which() == "driverMonitoringState"), False) + params_dict["IsRhdDetected"] = is_rhd + + if CP is not None: + if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: + params_dict["DisengageOnAccelerator"] = False + + if fingerprint is None: + if CP.fingerprintSource == "fw": + params_dict["CarParamsCache"] = CP.as_builder().to_bytes() + + if CP.openpilotLongitudinalControl: + params_dict["ExperimentalLongitudinalEnabled"] = True + + if CP.notCar: + params_dict["JoystickDebugMode"] = True + + return params_dict + + +def generate_environ_config(CP=None, fingerprint=None, log_dir=None) -> dict[str, Any]: + environ_dict = {} + if platform.system() != "Darwin": + environ_dict["PARAMS_ROOT"] = "/dev/shm/params" + if log_dir is not None: + environ_dict["LOG_ROOT"] = log_dir + + environ_dict["REPLAY"] = "1" + + # Regen or python process + if CP is not None and fingerprint is None: + if CP.fingerprintSource == "fw": + environ_dict['SKIP_FW_QUERY'] = "" + environ_dict['FINGERPRINT'] = "" + else: + environ_dict['SKIP_FW_QUERY'] = "1" + environ_dict['FINGERPRINT'] = CP.carFingerprint + elif fingerprint is not None: + environ_dict['SKIP_FW_QUERY'] = "1" + environ_dict['FINGERPRINT'] = fingerprint + else: + environ_dict["SKIP_FW_QUERY"] = "" + environ_dict["FINGERPRINT"] = "" + + return environ_dict + + +def check_openpilot_enabled(msgs: LogIterable) -> bool: + cur_enabled_count = 0 + max_enabled_count = 0 + for msg in msgs: + if msg.which() == "carParams": + if msg.carParams.notCar: + return True + elif msg.which() == "controlsState": + if msg.controlsState.active: + cur_enabled_count += 1 + else: + cur_enabled_count = 0 + max_enabled_count = max(max_enabled_count, cur_enabled_count) + + return max_enabled_count > int(10. / DT_CTRL) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit new file mode 100644 index 0000000..cb90463 --- /dev/null +++ b/selfdrive/test/process_replay/ref_commit @@ -0,0 +1 @@ +d544804a4fb54c0f160682b8f14af316a8383cd8 \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py new file mode 100644 index 0000000..ec3023c --- /dev/null +++ b/selfdrive/test/process_replay/regen.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +import os +import argparse +import time +import capnp +import numpy as np + +from typing import Any +from collections.abc import Iterable + +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, ProcessConfig, replay_process, get_process_config, \ + check_openpilot_enabled, get_custom_params_from_lr +from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_CAMERA_FRAME_SIZES +from openpilot.selfdrive.test.update_ci_routes import upload_route +from openpilot.tools.lib.route import Route +from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType +from openpilot.tools.lib.logreader import LogReader, LogIterable +from openpilot.tools.lib.helpers import save_log + + +class DummyFrameReader(BaseFrameReader): + def __init__(self, w: int, h: int, frame_count: int, pix_val: int): + self.pix_val = pix_val + self.w, self.h = w, h + self.frame_count = frame_count + self.frame_type = FrameType.raw + + def get(self, idx, count=1, pix_fmt="yuv420p"): + if pix_fmt == "rgb24": + shape = (self.h, self.w, 3) + elif pix_fmt == "nv12" or pix_fmt == "yuv420p": + shape = (int((self.h * self.w) * 3 / 2),) + else: + raise NotImplementedError + + return [np.full(shape, self.pix_val, dtype=np.uint8) for _ in range(count)] + + @staticmethod + def zero_dcamera(): + return DummyFrameReader(*DRIVER_CAMERA_FRAME_SIZES[("tici", "ar0231")], 1200, 0) + + +def regen_segment( + lr: LogIterable, frs: dict[str, Any] = None, + processes: Iterable[ProcessConfig] = CONFIGS, disable_tqdm: bool = False +) -> list[capnp._DynamicStructReader]: + all_msgs = sorted(lr, key=lambda m: m.logMonoTime) + custom_params = get_custom_params_from_lr(all_msgs) + + print("Replayed processes:", [p.proc_name for p in processes]) + print("\n\n", "*"*30, "\n\n", sep="") + + output_logs = replay_process(processes, all_msgs, frs, return_all_logs=True, custom_params=custom_params, disable_progress=disable_tqdm) + + return output_logs + + +def setup_data_readers( + route: str, sidx: int, use_route_meta: bool, + needs_driver_cam: bool = True, needs_road_cam: bool = True, dummy_driver_cam: bool = False +) -> tuple[LogReader, dict[str, Any]]: + if use_route_meta: + r = Route(route) + lr = LogReader(r.log_paths()[sidx]) + frs = {} + if needs_road_cam and len(r.camera_paths()) > sidx and r.camera_paths()[sidx] is not None: + frs['roadCameraState'] = FrameReader(r.camera_paths()[sidx]) + if needs_road_cam and len(r.ecamera_paths()) > sidx and r.ecamera_paths()[sidx] is not None: + frs['wideRoadCameraState'] = FrameReader(r.ecamera_paths()[sidx]) + if needs_driver_cam: + if dummy_driver_cam: + frs['driverCameraState'] = DummyFrameReader.zero_dcamera() + elif len(r.dcamera_paths()) > sidx and r.dcamera_paths()[sidx] is not None: + device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") + assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." + frs['driverCameraState'] = FrameReader(r.dcamera_paths()[sidx]) + else: + lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") + frs = {} + if needs_road_cam: + frs['roadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") + if next((True for m in lr if m.which() == "wideRoadCameraState"), False): + frs['wideRoadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/ecamera.hevc") + if needs_driver_cam: + if dummy_driver_cam: + frs['driverCameraState'] = DummyFrameReader.zero_dcamera() + else: + device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") + assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." + frs['driverCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/dcamera.hevc") + + return lr, frs + + +def regen_and_save( + route: str, sidx: int, processes: str | Iterable[str] = "all", outdir: str = FAKEDATA, + upload: bool = False, use_route_meta: bool = False, disable_tqdm: bool = False, dummy_driver_cam: bool = False +) -> str: + if not isinstance(processes, str) and not hasattr(processes, "__iter__"): + raise ValueError("whitelist_proc must be a string or iterable") + + if processes != "all": + if isinstance(processes, str): + raise ValueError(f"Invalid value for processes: {processes}") + + replayed_processes = [] + for d in processes: + cfg = get_process_config(d) + replayed_processes.append(cfg) + else: + replayed_processes = CONFIGS + + all_vision_pubs = {pub for cfg in replayed_processes for pub in cfg.vision_pubs} + lr, frs = setup_data_readers(route, sidx, use_route_meta, + needs_driver_cam="driverCameraState" in all_vision_pubs, + needs_road_cam="roadCameraState" in all_vision_pubs or "wideRoadCameraState" in all_vision_pubs, + dummy_driver_cam=dummy_driver_cam) + output_logs = regen_segment(lr, frs, replayed_processes, disable_tqdm=disable_tqdm) + + log_dir = os.path.join(outdir, time.strftime("%Y-%m-%d--%H-%M-%S--0", time.gmtime())) + rel_log_dir = os.path.relpath(log_dir) + rpath = os.path.join(log_dir, "rlog.bz2") + + os.makedirs(log_dir) + save_log(rpath, output_logs, compress=True) + + print("\n\n", "*"*30, "\n\n", sep="") + print("New route:", rel_log_dir, "\n") + + if not check_openpilot_enabled(output_logs): + raise Exception("Route did not engage for long enough") + + if upload: + upload_route(rel_log_dir) + + return rel_log_dir + + +if __name__ == "__main__": + def comma_separated_list(string): + return string.split(",") + + all_procs = [p.proc_name for p in CONFIGS] + parser = argparse.ArgumentParser(description="Generate new segments from old ones") + parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") + parser.add_argument("--outdir", help="log output dir", default=FAKEDATA) + parser.add_argument("--dummy-dcamera", action='store_true', help="Use dummy blank driver camera") + parser.add_argument("--whitelist-procs", type=comma_separated_list, default=all_procs, + help="Comma-separated whitelist of processes to regen (e.g. controlsd,radard)") + parser.add_argument("--blacklist-procs", type=comma_separated_list, default=[], + help="Comma-separated blacklist of processes to regen (e.g. controlsd,radard)") + parser.add_argument("route", type=str, help="The source route") + parser.add_argument("seg", type=int, help="Segment in source route") + args = parser.parse_args() + + blacklist_set = set(args.blacklist_procs) + processes = [p for p in args.whitelist_procs if p not in blacklist_set] + regen_and_save(args.route, args.seg, processes=processes, upload=args.upload, outdir=args.outdir, dummy_driver_cam=args.dummy_dcamera) diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py new file mode 100644 index 0000000..656a5b8 --- /dev/null +++ b/selfdrive/test/process_replay/regen_all.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import argparse +import concurrent.futures +import os +import random +import traceback +from tqdm import tqdm + +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.test.process_replay.regen import regen_and_save +from openpilot.selfdrive.test.process_replay.test_processes import FAKEDATA, source_segments as segments +from openpilot.tools.lib.route import SegmentName + + +def regen_job(segment, upload, disable_tqdm): + with OpenpilotPrefix(): + sn = SegmentName(segment[1]) + fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for _ in range(11)) + try: + relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, use_route_meta=False, + outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm, dummy_driver_cam=True) + relr = '|'.join(relr.split('/')[-2:]) + return f' ("{segment[0]}", "{relr}"), ' + except Exception as e: + err = f" {segment} failed: {str(e)}" + err += traceback.format_exc() + err += "\n\n" + return err + + +if __name__ == "__main__": + all_cars = {car for car, _ in segments} + + parser = argparse.ArgumentParser(description="Generate new segments from old ones") + parser.add_argument("-j", "--jobs", type=int, default=1) + parser.add_argument("--no-upload", action="store_true") + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, + help="Whitelist given cars from the test (e.g. HONDA)") + parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], + help="Blacklist given cars from the test (e.g. HONDA)") + args = parser.parse_args() + + tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) + tested_cars = {c.upper() for c in tested_cars} + tested_segments = [(car, segment) for car, segment in segments if car in tested_cars] + + with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: + p = pool.map(regen_job, tested_segments, [not args.no_upload] * len(tested_segments), [args.jobs > 1] * len(tested_segments)) + msg = "Copy these new segments into test_processes.py:" + for seg in tqdm(p, desc="Generating segments", total=len(tested_segments)): + msg += "\n" + str(seg) + print() + print() + print(msg) diff --git a/selfdrive/test/process_replay/test_debayer.py b/selfdrive/test/process_replay/test_debayer.py new file mode 100644 index 0000000..edf2cbd --- /dev/null +++ b/selfdrive/test/process_replay/test_debayer.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 +import os +import sys +import bz2 +import numpy as np + +import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip install pyopencl` + +from openpilot.system.hardware import PC, TICI +from openpilot.common.basedir import BASEDIR +from openpilot.tools.lib.openpilotci import BASE_URL +from openpilot.system.version import get_commit +from openpilot.system.camerad.snapshot.snapshot import yuv_to_rgb +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.filereader import FileReader + +TEST_ROUTE = "8345e3b82948d454|2022-05-04--13-45-33/0" + +FRAME_WIDTH = 1928 +FRAME_HEIGHT = 1208 +FRAME_STRIDE = 2896 + +UV_WIDTH = FRAME_WIDTH // 2 +UV_HEIGHT = FRAME_HEIGHT // 2 +UV_SIZE = UV_WIDTH * UV_HEIGHT + + +def get_frame_fn(ref_commit, test_route, tici=True): + return f"{test_route}_debayer{'_tici' if tici else ''}_{ref_commit}.bz2" + + +def bzip_frames(frames): + data = b'' + for y, u, v in frames: + data += y.tobytes() + data += u.tobytes() + data += v.tobytes() + return bz2.compress(data) + + +def unbzip_frames(url): + with FileReader(url) as f: + dat = f.read() + + data = bz2.decompress(dat) + + res = [] + for y_start in range(0, len(data), FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2): + u_start = y_start + FRAME_WIDTH * FRAME_HEIGHT + v_start = u_start + UV_SIZE + + y = np.frombuffer(data[y_start: u_start], dtype=np.uint8).reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = np.frombuffer(data[u_start: v_start], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) + v = np.frombuffer(data[v_start: v_start + UV_SIZE], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) + + res.append((y, u, v)) + + return res + + +def init_kernels(frame_offset=0): + ctx = cl.create_some_context(interactive=False) + + with open(os.path.join(BASEDIR, 'system/camerad/cameras/real_debayer.cl')) as f: + build_args = ' -cl-fast-relaxed-math -cl-denorms-are-zero -cl-single-precision-constant' + \ + f' -DFRAME_STRIDE={FRAME_STRIDE} -DRGB_WIDTH={FRAME_WIDTH} -DRGB_HEIGHT={FRAME_HEIGHT} -DFRAME_OFFSET={frame_offset} -DCAM_NUM=0' + if PC: + build_args += ' -DHALF_AS_FLOAT=1 -cl-std=CL2.0' + debayer_prg = cl.Program(ctx, f.read()).build(options=build_args) + + return ctx, debayer_prg + +def debayer_frame(ctx, debayer_prg, data, rgb=False): + q = cl.CommandQueue(ctx) + + yuv_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2, dtype=np.uint8) + + cam_g = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=data) + yuv_g = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2) + + local_worksize = (20, 20) if TICI else (4, 4) + ev1 = debayer_prg.debayer10(q, (UV_WIDTH, UV_HEIGHT), local_worksize, cam_g, yuv_g) + cl.enqueue_copy(q, yuv_buff, yuv_g, wait_for=[ev1]).wait() + cl.enqueue_barrier(q) + + y = yuv_buff[:FRAME_WIDTH*FRAME_HEIGHT].reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT:FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE].reshape((UV_HEIGHT, UV_WIDTH)) + v = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE:].reshape((UV_HEIGHT, UV_WIDTH)) + + if rgb: + return yuv_to_rgb(y, u, v) + else: + return y, u, v + + +def debayer_replay(lr): + ctx, debayer_prg = init_kernels() + + frames = [] + for m in lr: + if m.which() == 'roadCameraState': + cs = m.roadCameraState + if cs.image: + data = np.frombuffer(cs.image, dtype=np.uint8) + img = debayer_frame(ctx, debayer_prg, data) + + frames.append(img) + + return frames + + +if __name__ == "__main__": + update = "--update" in sys.argv + replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(replay_dir, "debayer_replay_ref_commit") + + # load logs + lr = list(LogReader(TEST_ROUTE)) + + # run replay + frames = debayer_replay(lr) + + # get diff + failed = False + diff = '' + yuv_i = ['y', 'u', 'v'] + if not update: + with open(ref_commit_fn) as f: + ref_commit = f.read().strip() + frame_fn = get_frame_fn(ref_commit, TEST_ROUTE, tici=TICI) + + try: + cmp_frames = unbzip_frames(BASE_URL + frame_fn) + + if len(frames) != len(cmp_frames): + failed = True + diff += 'amount of frames not equal\n' + + for i, (frame, cmp_frame) in enumerate(zip(frames, cmp_frames, strict=True)): + for j in range(3): + fr = frame[j] + cmp_f = cmp_frame[j] + if fr.shape != cmp_f.shape: + failed = True + diff += f'frame shapes not equal for ({i}, {yuv_i[j]})\n' + diff += f'{ref_commit}: {cmp_f.shape}\n' + diff += f'HEAD: {fr.shape}\n' + elif not np.array_equal(fr, cmp_f): + failed = True + if np.allclose(fr, cmp_f, atol=1): + diff += f'frames not equal for ({i}, {yuv_i[j]}), but are all close\n' + else: + diff += f'frames not equal for ({i}, {yuv_i[j]})\n' + + frame_diff = np.abs(np.subtract(fr, cmp_f)) + diff_len = len(np.nonzero(frame_diff)[0]) + if diff_len > 10000: + diff += f'different at a large amount of pixels ({diff_len})\n' + else: + diff += 'different at (frame, yuv, pixel, ref, HEAD):\n' + for k in zip(*np.nonzero(frame_diff), strict=True): + diff += f'{i}, {yuv_i[j]}, {k}, {cmp_f[k]}, {fr[k]}\n' + + if failed: + print(diff) + with open("debayer_diff.txt", "w") as f: + f.write(diff) + except Exception as e: + print(str(e)) + failed = True + + # upload new refs + if update or (failed and TICI): + from openpilot.tools.lib.openpilotci import upload_file + + print("Uploading new refs") + + frames_bzip = bzip_frames(frames) + + new_commit = get_commit() + frame_fn = os.path.join(replay_dir, get_frame_fn(new_commit, TEST_ROUTE, tici=TICI)) + with open(frame_fn, "wb") as f2: + f2.write(frames_bzip) + + try: + upload_file(frame_fn, os.path.basename(frame_fn)) + except Exception as e: + print("failed to upload", e) + + if update: + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) + + print("\nNew ref commit: ", new_commit) + + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py new file mode 100644 index 0000000..adff06f --- /dev/null +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import copy +from hypothesis import given, HealthCheck, Phase, settings +import hypothesis.strategies as st +from parameterized import parameterized +import unittest + +from cereal import log +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator +import openpilot.selfdrive.test.process_replay.process_replay as pr + +# These processes currently fail because of unrealistic data breaking assumptions +# that openpilot makes causing error with NaN, inf, int size, array indexing ... +# TODO: Make each one testable +NOT_TESTED = ['controlsd', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] + +TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED] + +class TestFuzzProcesses(unittest.TestCase): + + # TODO: make this faster and increase examples + @parameterized.expand(TEST_CASES) + @given(st.data()) + @settings(phases=[Phase.generate, Phase.target], max_examples=10, deadline=1000, suppress_health_check=[HealthCheck.too_slow, HealthCheck.data_too_large]) + def test_fuzz_process(self, proc_name, cfg, data): + msgs = FuzzyGenerator.get_random_event_msg(data.draw, events=cfg.pubs, real_floats=True) + lr = [log.Event.new_message(**m).as_reader() for m in msgs] + cfg.timeout = 5 + pr.replay_process(cfg, lr, fingerprint=TOYOTA.COROLLA_TSS2, disable_progress=True) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py new file mode 100644 index 0000000..88e46ab --- /dev/null +++ b/selfdrive/test/process_replay/test_processes.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +import argparse +import concurrent.futures +import os +import sys +from collections import defaultdict +from tqdm import tqdm +from typing import Any + +from openpilot.selfdrive.car.car_helpers import interface_names +from openpilot.tools.lib.openpilotci import get_url, upload_file +from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, check_openpilot_enabled, replay_process +from openpilot.system.version import get_commit +from openpilot.tools.lib.filereader import FileReader +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.helpers import save_log + +source_segments = [ + ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.BODY + ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA + ("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), # HYUNDAI.KIA_EV6 (+ QCOM GPS) + ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS + ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 + ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.COROLLA_TSS2 + ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.CIVIC (NIDEC) + ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.ACCORD (BOSCH) + ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.PACIFICA_2018_HYBRID + ("RAM", "17fc16d840fe9d21|2023-04-26--13-28-44--5"), # CHRYSLER.RAM_1500 + ("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"), # SUBARU.OUTBACK + ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.VOLT + ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.BOLT_EUV + ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL + ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF + ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.CX9_2021 + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1 + + # Enable when port is tested and dashcamOnly is no longer set + #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS + #("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.PASSAT_NMS +] + +segments = [ + ("BODY", "regen997DF2697CB|2023-10-30--23-14-29--0"), + ("HYUNDAI", "regen2A9D2A8E0B4|2023-10-30--23-13-34--0"), + ("HYUNDAI2", "regen6CA24BC3035|2023-10-30--23-14-28--0"), + ("TOYOTA", "regen5C019D76307|2023-10-30--23-13-31--0"), + ("TOYOTA2", "regen5DCADA88A96|2023-10-30--23-14-57--0"), + ("TOYOTA3", "regen7204CA3A498|2023-10-30--23-15-55--0"), + ("HONDA", "regen048F8FA0B24|2023-10-30--23-15-53--0"), + ("HONDA2", "regen7D2D3F82D5B|2023-10-30--23-15-55--0"), + ("CHRYSLER", "regen7125C42780C|2023-10-30--23-16-21--0"), + ("RAM", "regen2731F3213D2|2023-10-30--23-18-11--0"), + ("SUBARU", "regen86E4C1B4DDD|2023-10-30--23-18-14--0"), + ("GM", "regenF6393D64745|2023-10-30--23-17-18--0"), + ("GM2", "regen220F830C05B|2023-10-30--23-18-39--0"), + ("NISSAN", "regen4F671F7C435|2023-10-30--23-18-40--0"), + ("VOLKSWAGEN", "regen8BDFE7307A0|2023-10-30--23-19-36--0"), + ("MAZDA", "regen2E9F1A15FD5|2023-10-30--23-20-36--0"), + ("FORD", "regen6D39E54606E|2023-10-30--23-20-54--0"), +] + +# dashcamOnly makes don't need to be tested until a full port is done +excluded_interfaces = ["mock", "tesla"] + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" +REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit") +EXCLUDED_PROCS = {"modeld", "dmonitoringmodeld"} + + +def run_test_process(data): + segment, cfg, args, cur_log_fn, ref_log_path, lr_dat = data + res = None + if not args.upload_only: + lr = LogReader.from_bytes(lr_dat) + res, log_msgs = test_process(cfg, lr, segment, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs) + # save logs so we can upload when updating refs + save_log(cur_log_fn, log_msgs) + + if args.update_refs or args.upload_only: + print(f'Uploading: {os.path.basename(cur_log_fn)}') + assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}" + upload_file(cur_log_fn, os.path.basename(cur_log_fn)) + os.remove(cur_log_fn) + return (segment, cfg.proc_name, res) + + +def get_log_data(segment): + r, n = segment.rsplit("--", 1) + with FileReader(get_url(r, n)) as f: + return (segment, f.read()) + + +def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None): + if ignore_fields is None: + ignore_fields = [] + if ignore_msgs is None: + ignore_msgs = [] + + ref_log_msgs = list(LogReader(ref_log_path)) + + try: + log_msgs = replay_process(cfg, lr, disable_progress=True) + except Exception as e: + raise Exception("failed on segment: " + segment) from e + + # check to make sure openpilot is engaged in the route + if cfg.proc_name == "controlsd": + if not check_openpilot_enabled(log_msgs): + # FIXME: these segments should work, but the replay enabling logic is too brittle + if segment not in ("regen6CA24BC3035|2023-10-30--23-14-28--0", "regen7D2D3F82D5B|2023-10-30--23-15-55--0"): + return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs + + try: + return compare_logs(ref_log_msgs, log_msgs, ignore_fields + cfg.ignore, ignore_msgs, cfg.tolerance), log_msgs + except Exception as e: + return str(e), log_msgs + + +if __name__ == "__main__": + all_cars = {car for car, _ in segments} + all_procs = {cfg.proc_name for cfg in CONFIGS if cfg.proc_name not in EXCLUDED_PROCS} + + cpu_count = os.cpu_count() or 1 + + parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") + parser.add_argument("--whitelist-procs", type=str, nargs="*", default=all_procs, + help="Whitelist given processes from the test (e.g. controlsd)") + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, + help="Whitelist given cars from the test (e.g. HONDA)") + parser.add_argument("--blacklist-procs", type=str, nargs="*", default=[], + help="Blacklist given processes from the test (e.g. controlsd)") + parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], + help="Blacklist given cars from the test (e.g. HONDA)") + parser.add_argument("--ignore-fields", type=str, nargs="*", default=[], + help="Extra fields or msgs to ignore (e.g. carState.events)") + parser.add_argument("--ignore-msgs", type=str, nargs="*", default=[], + help="Msgs to ignore (e.g. carEvents)") + parser.add_argument("--update-refs", action="store_true", + help="Updates reference logs using current commit") + parser.add_argument("--upload-only", action="store_true", + help="Skips testing processes and uploads logs from previous test run") + parser.add_argument("-j", "--jobs", type=int, default=max(cpu_count - 2, 1), + help="Max amount of parallel jobs") + args = parser.parse_args() + + tested_procs = set(args.whitelist_procs) - set(args.blacklist_procs) + tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) + tested_cars = {c.upper() for c in tested_cars} + + full_test = (tested_procs == all_procs) and (tested_cars == all_cars) and all(len(x) == 0 for x in (args.ignore_fields, args.ignore_msgs)) + upload = args.update_refs or args.upload_only + os.makedirs(os.path.dirname(FAKEDATA), exist_ok=True) + + if upload: + assert full_test, "Need to run full test when updating refs" + + try: + with open(REF_COMMIT_FN) as f: + ref_commit = f.read().strip() + except FileNotFoundError: + print("Couldn't find reference commit") + sys.exit(1) + + cur_commit = get_commit() + if not cur_commit: + raise Exception("Couldn't get current commit") + + print(f"***** testing against commit {ref_commit} *****") + + # check to make sure all car brands are tested + if full_test: + untested = (set(interface_names) - set(excluded_interfaces)) - {c.lower() for c in tested_cars} + assert len(untested) == 0, f"Cars missing routes: {str(untested)}" + + log_paths: defaultdict[str, dict[str, dict[str, str]]] = defaultdict(lambda: defaultdict(dict)) + with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: + if not args.upload_only: + download_segments = [seg for car, seg in segments if car in tested_cars] + log_data: dict[str, LogReader] = {} + p1 = pool.map(get_log_data, download_segments) + for segment, lr in tqdm(p1, desc="Getting Logs", total=len(download_segments)): + log_data[segment] = lr + + pool_args: Any = [] + for car_brand, segment in segments: + if car_brand not in tested_cars: + continue + + for cfg in CONFIGS: + if cfg.proc_name not in tested_procs: + continue + + cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2") + if args.update_refs: # reference logs will not exist if routes were just regenerated + ref_log_path = get_url(*segment.rsplit("--", 1)) + else: + ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2") + ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) + + dat = None if args.upload_only else log_data[segment] + pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat)) + + log_paths[segment][cfg.proc_name]['ref'] = ref_log_path + log_paths[segment][cfg.proc_name]['new'] = cur_log_fn + + results: Any = defaultdict(dict) + p2 = pool.map(run_test_process, pool_args) + for (segment, proc, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)): + if not args.upload_only: + results[segment][proc] = result + + diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit) + if not upload: + with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f: + f.write(diff_long) + print(diff_short) + + if failed: + print("TEST FAILED") + print("\n\nTo push the new reference logs for this commit run:") + print("./test_processes.py --upload-only") + else: + print("TEST SUCCEEDED") + + else: + with open(REF_COMMIT_FN, "w") as f: + f.write(cur_commit) + print(f"\n\nUpdated reference logs for commit: {cur_commit}") + + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/test_regen.py b/selfdrive/test/process_replay/test_regen.py new file mode 100644 index 0000000..41d67ea --- /dev/null +++ b/selfdrive/test/process_replay/test_regen.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 + +import unittest + +from parameterized import parameterized + +from openpilot.selfdrive.test.process_replay.regen import regen_segment, DummyFrameReader +from openpilot.selfdrive.test.process_replay.process_replay import check_openpilot_enabled +from openpilot.tools.lib.openpilotci import get_url +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.framereader import FrameReader + +TESTED_SEGMENTS = [ + ("PRIUS_C2", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA PRIUS 2017: NEO, pandaStateDEPRECATED, no peripheralState, sensorEventsDEPRECATED + # Enable these once regen on CI becomes faster or use them for different tests running controlsd in isolation + # ("MAZDA_C3", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.CX9_2021: TICI, incomplete managerState + # ("FORD_C3", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1: TICI +] + + +def ci_setup_data_readers(route, sidx): + lr = LogReader(get_url(route, sidx, "rlog")) + frs = { + 'roadCameraState': FrameReader(get_url(route, sidx, "fcamera")), + 'driverCameraState': DummyFrameReader.zero_dcamera() + } + if next((True for m in lr if m.which() == "wideRoadCameraState"), False): + frs["wideRoadCameraState"] = FrameReader(get_url(route, sidx, "ecamera")) + + return lr, frs + + +class TestRegen(unittest.TestCase): + @parameterized.expand(TESTED_SEGMENTS) + def test_engaged(self, case_name, segment): + route, sidx = segment.rsplit("--", 1) + lr, frs = ci_setup_data_readers(route, sidx) + output_logs = regen_segment(lr, frs, disable_tqdm=True) + + engaged = check_openpilot_enabled(output_logs) + self.assertTrue(engaged, f"openpilot not engaged in {case_name}") + + +if __name__=='__main__': + unittest.main() diff --git a/selfdrive/test/process_replay/vision_meta.py b/selfdrive/test/process_replay/vision_meta.py new file mode 100644 index 0000000..9bfe214 --- /dev/null +++ b/selfdrive/test/process_replay/vision_meta.py @@ -0,0 +1,43 @@ +from collections import namedtuple +from cereal.visionipc import VisionStreamType +from openpilot.common.realtime import DT_MDL, DT_DMON +from openpilot.common.transformations.camera import DEVICE_CAMERAS + +VideoStreamMeta = namedtuple("VideoStreamMeta", ["camera_state", "encode_index", "stream", "dt", "frame_sizes"]) +ROAD_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} +WIDE_ROAD_CAMERA_FRAME_SIZES = {k: (v.ecam.width, v.ecam.height) for k, v in DEVICE_CAMERAS.items() if v.ecam is not None} +DRIVER_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} +VIPC_STREAM_METADATA = [ + # metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes) + ("roadCameraState", "roadEncodeIdx", VisionStreamType.VISION_STREAM_ROAD, DT_MDL, ROAD_CAMERA_FRAME_SIZES), + ("wideRoadCameraState", "wideRoadEncodeIdx", VisionStreamType.VISION_STREAM_WIDE_ROAD, DT_MDL, WIDE_ROAD_CAMERA_FRAME_SIZES), + ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_CAMERA_FRAME_SIZES), +] + + +def meta_from_camera_state(state): + meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[0] == state), None) + return meta + + +def meta_from_encode_index(encode_index): + meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[1] == encode_index), None) + return meta + + +def meta_from_stream_type(stream_type): + meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[2] == stream_type), None) + return meta + + +def available_streams(lr=None): + if lr is None: + return [VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA] + + result = [] + for meta in VIPC_STREAM_METADATA: + has_cam_state = next((True for m in lr if m.which() == meta[0]), False) + if has_cam_state: + result.append(VideoStreamMeta(*meta)) + + return result diff --git a/selfdrive/test/profiling/.gitignore b/selfdrive/test/profiling/.gitignore new file mode 100644 index 0000000..76acac7 --- /dev/null +++ b/selfdrive/test/profiling/.gitignore @@ -0,0 +1,2 @@ +cachegrind.out.* +*.prof diff --git a/selfdrive/test/profiling/__init__.py b/selfdrive/test/profiling/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/test/profiling/lib.py b/selfdrive/test/profiling/lib.py new file mode 100644 index 0000000..7f3b012 --- /dev/null +++ b/selfdrive/test/profiling/lib.py @@ -0,0 +1,91 @@ +from collections import defaultdict, deque +from cereal.services import SERVICE_LIST +import cereal.messaging as messaging +import capnp + + +class ReplayDone(Exception): + pass + + +class SubSocket(): + def __init__(self, msgs, trigger): + self.i = 0 + self.trigger = trigger + self.msgs = [m.as_builder().to_bytes() for m in msgs if m.which() == trigger] + self.max_i = len(self.msgs) - 1 + + def receive(self, non_blocking=False): + if non_blocking: + return None + + if self.i == self.max_i: + raise ReplayDone + + while True: + msg = self.msgs[self.i] + self.i += 1 + return msg + + +class PubSocket(): + def send(self, data): + pass + + +class SubMaster(messaging.SubMaster): + def __init__(self, msgs, trigger, services, check_averag_freq=False): + self.frame = 0 + self.data = {} + self.ignore_alive = [] + + self.alive = {s: True for s in services} + self.updated = {s: False for s in services} + self.rcv_time = {s: 0. for s in services} + self.rcv_frame = {s: 0 for s in services} + self.valid = {s: True for s in services} + self.freq_ok = {s: True for s in services} + self.recv_dts = {s: deque([0.0] * messaging.AVG_FREQ_HISTORY, maxlen=messaging.AVG_FREQ_HISTORY) for s in services} + self.logMonoTime = {} + self.sock = {} + self.freq = {} + self.check_average_freq = check_averag_freq + self.non_polled_services = [] + self.ignore_average_freq = [] + + # TODO: specify multiple triggers for service like plannerd that poll on more than one service + cur_msgs = [] + self.msgs = [] + msgs = [m for m in msgs if m.which() in services] + + for msg in msgs: + cur_msgs.append(msg) + if msg.which() == trigger: + self.msgs.append(cur_msgs) + cur_msgs = [] + + self.msgs = list(reversed(self.msgs)) + + for s in services: + self.freq[s] = SERVICE_LIST[s].frequency + try: + data = messaging.new_message(s) + except capnp.lib.capnp.KjException: + # lists + data = messaging.new_message(s, 0) + + self.data[s] = getattr(data, s) + self.logMonoTime[s] = 0 + self.sock[s] = SubSocket(msgs, s) + + def update(self, timeout=None): + if not len(self.msgs): + raise ReplayDone + + cur_msgs = self.msgs.pop() + self.update_msgs(cur_msgs[0].logMonoTime, self.msgs.pop()) + + +class PubMaster(messaging.PubMaster): + def __init__(self): + self.sock = defaultdict(PubSocket) diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py new file mode 100644 index 0000000..6571825 --- /dev/null +++ b/selfdrive/test/profiling/profiler.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +import os +import sys +import cProfile +import pprofile +import pyprof2calltree + +from openpilot.common.params import Params +from openpilot.tools.lib.logreader import LogReader +from openpilot.selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.volkswagen.values import CAR as VW + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" + +CARS = { + 'toyota': ("0982d79ebb0de295|2021-01-03--20-03-36/6", TOYOTA.RAV4), + 'honda': ("0982d79ebb0de295|2021-01-08--10-13-10/6", HONDA.CIVIC), + "vw": ("ef895f46af5fd73f|2021-05-22--14-06-35/6", VW.AUDI_A3_MK3), +} + + +def get_inputs(msgs, process, fingerprint): + for config in CONFIGS: + if config.proc_name == process: + sub_socks = list(config.pubs) + trigger = sub_socks[0] + break + + # some procs block on CarParams + for msg in msgs: + if msg.which() == 'carParams': + m = msg.as_builder() + m.carParams.carFingerprint = fingerprint + Params().put("CarParams", m.carParams.copy().to_bytes()) + break + + sm = SubMaster(msgs, trigger, sub_socks) + pm = PubMaster() + if 'can' in sub_socks: + can_sock = SubSocket(msgs, 'can') + else: + can_sock = None + return sm, pm, can_sock + + +def profile(proc, func, car='toyota'): + segment, fingerprint = CARS[car] + segment = segment.replace('|', '/') + rlog_url = f"{BASE_URL}{segment}/rlog.bz2" + msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1")) + + os.environ['FINGERPRINT'] = fingerprint + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['REPLAY'] = "1" + + def run(sm, pm, can_sock): + try: + if can_sock is not None: + func(sm, pm, can_sock) + else: + func(sm, pm) + except ReplayDone: + pass + + # Statistical + sm, pm, can_sock = get_inputs(msgs, proc, fingerprint) + with pprofile.StatisticalProfile()(period=0.00001) as pr: + run(sm, pm, can_sock) + pr.dump_stats(f'cachegrind.out.{proc}_statistical') + + # Deterministic + sm, pm, can_sock = get_inputs(msgs, proc, fingerprint) + with cProfile.Profile() as pr: + run(sm, pm, can_sock) + pyprof2calltree.convert(pr.getstats(), f'cachegrind.out.{proc}_deterministic') + + +if __name__ == '__main__': + from openpilot.selfdrive.controls.controlsd import main as controlsd_thread + from openpilot.selfdrive.locationd.paramsd import main as paramsd_thread + from openpilot.selfdrive.controls.plannerd import main as plannerd_thread + + procs = { + 'controlsd': controlsd_thread, + 'paramsd': paramsd_thread, + 'plannerd': plannerd_thread, + } + + proc = sys.argv[1] + if proc not in procs: + print(f"{proc} not available") + sys.exit(0) + else: + profile(proc, procs[proc]) diff --git a/selfdrive/test/scons_build_test.sh b/selfdrive/test/scons_build_test.sh new file mode 100644 index 0000000..a3b33f7 --- /dev/null +++ b/selfdrive/test/scons_build_test.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +SCRIPT_DIR=$(dirname "$0") +BASEDIR=$(realpath "$SCRIPT_DIR/../../") +cd $BASEDIR + +# tests that our build system's dependencies are configured properly, +# needs a machine with lots of cores +scons --clean +scons --no-cache --random -j$(nproc) \ No newline at end of file diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh new file mode 100755 index 0000000..5c85312 --- /dev/null +++ b/selfdrive/test/setup_device_ci.sh @@ -0,0 +1,120 @@ +#!/usr/bin/bash + +set -e + +if [ -z "$SOURCE_DIR" ]; then + echo "SOURCE_DIR must be set" + exit 1 +fi + +if [ -z "$GIT_COMMIT" ]; then + echo "GIT_COMMIT must be set" + exit 1 +fi + +if [ -z "$TEST_DIR" ]; then + echo "TEST_DIR must be set" + exit 1 +fi + +umount /data/safe_staging/merged/ || true +sudo umount /data/safe_staging/merged/ || true +rm -rf /data/safe_staging/* || true + +CONTINUE_PATH="/data/continue.sh" +tee $CONTINUE_PATH << EOF +#!/usr/bin/bash + +sudo abctl --set_success + +# patch sshd config +sudo mount -o rw,remount / +sudo sed -i "s,/data/params/d/GithubSshKeys,/usr/comma/setup_keys," /etc/ssh/sshd_config +sudo systemctl daemon-reload +sudo systemctl restart ssh +sudo systemctl restart NetworkManager +sudo systemctl disable ssh-param-watcher.path +sudo systemctl disable ssh-param-watcher.service +sudo mount -o ro,remount / + +while true; do + if ! sudo systemctl is-active -q ssh; then + sudo systemctl start ssh + fi + + #if ! pgrep -f 'ciui.py' > /dev/null 2>&1; then + # echo 'starting UI' + # cp $SOURCE_DIR/selfdrive/test/ciui.py /data/ + # /data/ciui.py & + #fi + + sleep 5s +done + +sleep infinity +EOF +chmod +x $CONTINUE_PATH + +safe_checkout() { + # completely clean TEST_DIR + + cd $SOURCE_DIR + + # cleanup orphaned locks + find .git -type f -name "*.lock" -exec rm {} + + + git reset --hard + git fetch --no-tags --no-recurse-submodules -j4 --verbose --depth 1 origin $GIT_COMMIT + find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; + git reset --hard $GIT_COMMIT + git checkout $GIT_COMMIT + git clean -xdff + git submodule sync + git submodule update --init --recursive + git submodule foreach --recursive "git reset --hard && git clean -xdff" + + git lfs pull + (ulimit -n 65535 && git lfs prune) + + echo "git checkout done, t=$SECONDS" + du -hs $SOURCE_DIR $SOURCE_DIR/.git + + rsync -a --delete $SOURCE_DIR $TEST_DIR +} + +unsafe_checkout() { + # checkout directly in test dir, leave old build products + + cd $TEST_DIR + + # cleanup orphaned locks + find .git -type f -name "*.lock" -exec rm {} + + + git fetch --no-tags --no-recurse-submodules -j8 --verbose --depth 1 origin $GIT_COMMIT + git checkout --force --no-recurse-submodules $GIT_COMMIT + git reset --hard $GIT_COMMIT + git clean -df + git submodule sync + git submodule update --init --recursive + git submodule foreach --recursive "git reset --hard && git clean -df" + + git lfs pull + (ulimit -n 65535 && git lfs prune) +} + +export GIT_PACK_THREADS=8 + +# set up environment +if [ ! -d "$SOURCE_DIR" ]; then + git clone https://github.com/commaai/openpilot.git $SOURCE_DIR +fi + +if [ ! -z "$UNSAFE" ]; then + echo "doing unsafe checkout" + unsafe_checkout +else + echo "doing safe checkout" + safe_checkout +fi + +echo "$TEST_DIR synced with $GIT_COMMIT, t=$SECONDS" diff --git a/selfdrive/test/setup_vsound.sh b/selfdrive/test/setup_vsound.sh new file mode 100644 index 0000000..a6601d0 --- /dev/null +++ b/selfdrive/test/setup_vsound.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +{ + #start pulseaudio daemon + sudo pulseaudio -D + + # create a virtual null audio and set it to default device + sudo pactl load-module module-null-sink sink_name=virtual_audio + sudo pactl set-default-sink virtual_audio +} > /dev/null 2>&1 diff --git a/selfdrive/test/setup_xvfb.sh b/selfdrive/test/setup_xvfb.sh new file mode 100644 index 0000000..692b84d --- /dev/null +++ b/selfdrive/test/setup_xvfb.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash + +# Sets up a virtual display for running map renderer and simulator without an X11 display + +DISP_ID=99 +export DISPLAY=:$DISP_ID + +sudo Xvfb $DISPLAY -screen 0 2160x1080x24 2>/dev/null & + +# check for x11 socket for the specified display ID +while [ ! -S /tmp/.X11-unix/X$DISP_ID ] +do + echo "Waiting for Xvfb..." + sleep 1 +done + +touch ~/.Xauthority +export XDG_SESSION_TYPE="x11" +xset -q \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py new file mode 100755 index 0000000..250534b --- /dev/null +++ b/selfdrive/test/test_onroad.py @@ -0,0 +1,427 @@ +#!/usr/bin/env python3 +import bz2 +import math +import json +import os +import pathlib +import psutil +import pytest +import shutil +import subprocess +import time +import numpy as np +import unittest +from collections import Counter, defaultdict +from functools import cached_property +from pathlib import Path + +from cereal import car +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.basedir import BASEDIR +from openpilot.common.timeout import Timeout +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.events import EVENTS, ET +from openpilot.system.hardware import HARDWARE +from openpilot.selfdrive.test.helpers import set_params_enabled, release_only +from openpilot.system.hardware.hw import Paths +from openpilot.tools.lib.logreader import LogReader + +# Baseline CPU usage by process +PROCS = { + "selfdrive.controls.controlsd": 46.0, + "./loggerd": 14.0, + "./encoderd": 17.0, + "./camerad": 14.5, + "./locationd": 11.0, + "./mapsd": (0.5, 10.0), + "selfdrive.controls.plannerd": 11.0, + "./ui": 18.0, + "selfdrive.locationd.paramsd": 9.0, + "./sensord": 7.0, + "selfdrive.controls.radard": 7.0, + "selfdrive.modeld.modeld": 13.0, + "selfdrive.modeld.dmonitoringmodeld": 8.0, + "selfdrive.modeld.navmodeld": 1.0, + "selfdrive.thermald.thermald": 3.87, + "selfdrive.locationd.calibrationd": 2.0, + "selfdrive.locationd.torqued": 5.0, + "selfdrive.ui.soundd": 3.5, + "selfdrive.monitoring.dmonitoringd": 4.0, + "./proclogd": 1.54, + "system.logmessaged": 0.2, + "selfdrive.tombstoned": 0, + "./logcatd": 0, + "system.micd": 6.0, + "system.timed": 0, + "selfdrive.boardd.pandad": 0, + "selfdrive.statsd": 0.4, + "selfdrive.navd.navd": 0.4, + "system.loggerd.uploader": (0.5, 15.0), + "system.loggerd.deleter": 0.1, +} + +PROCS.update({ + "tici": { + "./boardd": 4.0, + "./ubloxd": 0.02, + "system.ubloxd.pigeond": 6.0, + }, + "tizi": { + "./boardd": 19.0, + "system.qcomgpsd.qcomgpsd": 1.0, + } +}.get(HARDWARE.get_device_type(), {})) + +TIMINGS = { + # rtols: max/min, rsd + "can": [2.5, 0.35], + "pandaStates": [2.5, 0.35], + "peripheralState": [2.5, 0.35], + "sendcan": [2.5, 0.35], + "carState": [2.5, 0.35], + "carControl": [2.5, 0.35], + "controlsState": [2.5, 0.35], + "longitudinalPlan": [2.5, 0.5], + "roadCameraState": [2.5, 0.35], + "driverCameraState": [2.5, 0.35], + "modelV2": [2.5, 0.35], + "driverStateV2": [2.5, 0.40], + "navModel": [2.5, 0.35], + "mapRenderState": [2.5, 0.35], + "liveLocationKalman": [2.5, 0.35], + "wideRoadCameraState": [1.5, 0.35], +} + + +def cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +@pytest.mark.tici +class TestOnroad(unittest.TestCase): + + @classmethod + def setUpClass(cls): + if "DEBUG" in os.environ: + segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(Paths.log_root()).iterdir()) + segs = sorted(segs, key=lambda x: x.stat().st_mtime) + print(segs[-3]) + cls.lr = list(LogReader(os.path.join(segs[-3], "rlog"))) + return + + # setup env + params = Params() + params.remove("CurrentRoute") + set_params_enabled() + os.environ['REPLAY'] = '1' + os.environ['TESTING_CLOSET'] = '1' + if os.path.exists(Paths.log_root()): + shutil.rmtree(Paths.log_root()) + + # start manager and run openpilot for a minute + proc = None + try: + manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") + proc = subprocess.Popen(["python", manager_path]) + + sm = messaging.SubMaster(['carState']) + with Timeout(150, "controls didn't start"): + while sm.recv_frame['carState'] < 0: + sm.update(1000) + + # make sure we get at least two full segments + route = None + cls.segments = [] + with Timeout(300, "timed out waiting for logs"): + while route is None: + route = params.get("CurrentRoute", encoding="utf-8") + time.sleep(0.1) + + while len(cls.segments) < 3: + segs = set() + if Path(Paths.log_root()).exists(): + segs = set(Path(Paths.log_root()).glob(f"{route}--*")) + cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1])) + time.sleep(2) + + # chop off last, incomplete segment + cls.segments = cls.segments[:-1] + + finally: + cls.gpu_procs = {psutil.Process(int(f.name)).name() for f in pathlib.Path('/sys/devices/virtual/kgsl/kgsl/proc/').iterdir() if f.is_dir()} + + if proc is not None: + proc.terminate() + if proc.wait(60) is None: + proc.kill() + + cls.lrs = [list(LogReader(os.path.join(str(s), "rlog"))) for s in cls.segments] + + # use the second segment by default as it's the first full segment + cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog"))) + cls.log_path = cls.segments[1] + + cls.log_sizes = {} + for f in cls.log_path.iterdir(): + assert f.is_file() + cls.log_sizes[f] = f.stat().st_size / 1e6 + if f.name in ("qlog", "rlog"): + with open(f, 'rb') as ff: + cls.log_sizes[f] = len(bz2.compress(ff.read())) / 1e6 + + + @cached_property + def service_msgs(self): + msgs = defaultdict(list) + for m in self.lr: + msgs[m.which()].append(m) + return msgs + + def test_service_frequencies(self): + for s, msgs in self.service_msgs.items(): + if s in ('initData', 'sentinel'): + continue + + # skip gps services for now + if s in ('ubloxGnss', 'ubloxRaw', 'gnssMeasurements', 'gpsLocation', 'gpsLocationExternal', 'qcomGnss'): + continue + + with self.subTest(service=s): + assert len(msgs) >= math.floor(SERVICE_LIST[s].frequency*55) + + def test_cloudlog_size(self): + msgs = [m for m in self.lr if m.which() == 'logMessage'] + + total_size = sum(len(m.as_builder().to_bytes()) for m in msgs) + self.assertLess(total_size, 3.5e5) + + cnt = Counter(json.loads(m.logMessage)['filename'] for m in msgs) + big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.] + self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}") + + def test_log_sizes(self): + for f, sz in self.log_sizes.items(): + if f.name == "qcamera.ts": + assert 2.15 < sz < 2.35 + elif f.name == "qlog": + assert 0.7 < sz < 1.0 + elif f.name == "rlog": + assert 5 < sz < 50 + elif f.name.endswith('.hevc'): + assert 70 < sz < 77 + else: + raise NotImplementedError + + def test_ui_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "-------------- UI Draw Timing ------------------\n" + result += "------------------------------------------------\n" + + ts = [m.uiDebug.drawTimeMillis for m in self.service_msgs['uiDebug']] + result += f"min {min(ts):.2f}ms\n" + result += f"max {max(ts):.2f}ms\n" + result += f"std {np.std(ts):.2f}ms\n" + result += f"mean {np.mean(ts):.2f}ms\n" + result += "------------------------------------------------\n" + print(result) + + self.assertLess(max(ts), 250.) + self.assertLess(np.mean(ts), 10.) + #self.assertLess(np.std(ts), 5.) + + # some slow frames are expected since camerad/modeld can preempt ui + veryslow = [x for x in ts if x > 40.] + assert len(veryslow) < 5, f"Too many slow frame draw times: {veryslow}" + + def test_cpu_usage(self): + result = "\n" + result += "------------------------------------------------\n" + result += "------------------ CPU Usage -------------------\n" + result += "------------------------------------------------\n" + + plogs_by_proc = defaultdict(list) + for pl in self.service_msgs['procLog']: + for x in pl.procLog.procs: + if len(x.cmdline) > 0: + n = list(x.cmdline)[0] + plogs_by_proc[n].append(x) + print(plogs_by_proc.keys()) + + cpu_ok = True + dt = (self.service_msgs['procLog'][-1].logMonoTime - self.service_msgs['procLog'][0].logMonoTime) / 1e9 + for proc_name, expected_cpu in PROCS.items(): + + err = "" + cpu_usage = 0. + x = plogs_by_proc[proc_name] + if len(x) > 2: + cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) + cpu_usage = cpu_time / dt * 100. + + if isinstance(expected_cpu, tuple): + exp = str(expected_cpu) + minn, maxx = expected_cpu + else: + exp = f"{expected_cpu:5.2f}" + minn = min(expected_cpu * 0.65, max(expected_cpu - 1.0, 0.0)) + maxx = max(expected_cpu * 1.15, expected_cpu + 5.0) + + if cpu_usage > maxx: + err = "using more CPU than expected" + elif cpu_usage < minn: + err = "using less CPU than expected" + else: + err = "NO METRICS FOUND" + + result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({exp}%) {err}\n" + if len(err) > 0: + cpu_ok = False + + # Ensure there's no missing procs + all_procs = {p.name for p in self.service_msgs['managerState'][0].managerState.processes if p.shouldBeRunning} + for p in all_procs: + with self.subTest(proc=p): + assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}" + + result += "------------------------------------------------\n" + print(result) + + self.assertTrue(cpu_ok) + + def test_memory_usage(self): + mems = [m.deviceState.memoryUsagePercent for m in self.service_msgs['deviceState']] + print("Memory usage: ", mems) + + # check for big leaks. note that memory usage is + # expected to go up while the MSGQ buffers fill up + self.assertLessEqual(max(mems) - min(mems), 3.0) + + def test_gpu_usage(self): + self.assertEqual(self.gpu_procs, {"weston", "ui", "camerad", "selfdrive.modeld.modeld"}) + + def test_camera_processing_time(self): + result = "\n" + result += "------------------------------------------------\n" + result += "-------------- Debayer Timing ------------------\n" + result += "------------------------------------------------\n" + + ts = [getattr(m, m.which()).processingTime for m in self.lr if 'CameraState' in m.which()] + self.assertLess(min(ts), 0.025, f"high execution time: {min(ts)}") + result += f"execution time: min {min(ts):.5f}s\n" + result += f"execution time: max {max(ts):.5f}s\n" + result += f"execution time: mean {np.mean(ts):.5f}s\n" + result += "------------------------------------------------\n" + print(result) + + @unittest.skip("TODO: enable once timings are fixed") + def test_camera_frame_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- SoF Timing ------------------\n" + result += "------------------------------------------------\n" + for name in ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']: + ts = [getattr(m, m.which()).timestampSof for m in self.lr if name in m.which()] + d_ms = np.diff(ts) / 1e6 + d50 = np.abs(d_ms-50) + self.assertLess(max(d50), 1.0, f"high sof delta vs 50ms: {max(d50)}") + result += f"{name} sof delta vs 50ms: min {min(d50):.5f}s\n" + result += f"{name} sof delta vs 50ms: max {max(d50):.5f}s\n" + result += f"{name} sof delta vs 50ms: mean {d50.mean():.5f}s\n" + result += "------------------------------------------------\n" + print(result) + + def test_mpc_execution_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- MPC Timing ------------------\n" + result += "------------------------------------------------\n" + + cfgs = [("longitudinalPlan", 0.05, 0.05),] + for (s, instant_max, avg_max) in cfgs: + ts = [getattr(m, s).solverExecutionTime for m in self.service_msgs[s]] + self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") + self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") + result += f"'{s}' execution time: min {min(ts):.5f}s\n" + result += f"'{s}' execution time: max {max(ts):.5f}s\n" + result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" + result += "------------------------------------------------\n" + print(result) + + def test_model_execution_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- Model Timing -----------------\n" + result += "------------------------------------------------\n" + # TODO: this went up when plannerd cpu usage increased, why? + cfgs = [ + ("modelV2", 0.050, 0.036), + ("driverStateV2", 0.050, 0.026), + ] + for (s, instant_max, avg_max) in cfgs: + ts = [getattr(m, s).modelExecutionTime for m in self.service_msgs[s]] + self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") + self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") + result += f"'{s}' execution time: min {min(ts):.5f}s\n" + result += f"'{s}' execution time: max {max(ts):.5f}s\n" + result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" + result += "------------------------------------------------\n" + print(result) + + def test_timings(self): + passed = True + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- Service Timings --------------\n" + result += "------------------------------------------------\n" + for s, (maxmin, rsd) in TIMINGS.items(): + msgs = [m.logMonoTime for m in self.service_msgs[s]] + if not len(msgs): + raise Exception(f"missing {s}") + + ts = np.diff(msgs) / 1e9 + dt = 1 / SERVICE_LIST[s].frequency + + try: + np.testing.assert_allclose(np.mean(ts), dt, rtol=0.03, err_msg=f"{s} - failed mean timing check") + np.testing.assert_allclose([np.max(ts), np.min(ts)], dt, rtol=maxmin, err_msg=f"{s} - failed max/min timing check") + except Exception as e: + result += str(e) + "\n" + passed = False + + if np.std(ts) / dt > rsd: + result += f"{s} - failed RSD timing check\n" + passed = False + + result += f"{s.ljust(40)}: {np.array([np.mean(ts), np.max(ts), np.min(ts)])*1e3}\n" + result += f"{''.ljust(40)} {np.max(np.absolute([np.max(ts)/dt, np.min(ts)/dt]))} {np.std(ts)/dt}\n" + result += "="*67 + print(result) + self.assertTrue(passed) + + @release_only + def test_startup(self): + startup_alert = None + for msg in self.lrs[0]: + # can't use onroadEvents because the first msg can be dropped while loggerd is starting up + if msg.which() == "controlsState": + startup_alert = msg.controlsState.alertText1 + break + expected = EVENTS[car.CarEvent.EventName.startup][ET.PERMANENT].alert_text_1 + self.assertEqual(startup_alert, expected, "wrong startup alert") + + def test_engagable(self): + no_entries = Counter() + for m in self.service_msgs['onroadEvents']: + for evt in m.onroadEvents: + if evt.noEntry: + no_entries[evt.name] += 1 + + eng = [m.controlsState.engageable for m in self.service_msgs['controlsState']] + assert all(eng), \ + f"Not engageable for whole segment:\n- controlsState.engageable: {Counter(eng)}\n- No entry events: {no_entries}" + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/test_time_to_onroad.py b/selfdrive/test/test_time_to_onroad.py new file mode 100755 index 0000000..a3f803e --- /dev/null +++ b/selfdrive/test/test_time_to_onroad.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 +import os +import pytest +import time +import subprocess + +import cereal.messaging as messaging +from openpilot.common.basedir import BASEDIR +from openpilot.common.timeout import Timeout +from openpilot.selfdrive.test.helpers import set_params_enabled + + +@pytest.mark.tici +def test_time_to_onroad(): + # launch + set_params_enabled() + manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") + proc = subprocess.Popen(["python", manager_path]) + + start_time = time.monotonic() + sm = messaging.SubMaster(['controlsState', 'deviceState', 'onroadEvents', 'sendcan']) + try: + # wait for onroad. timeout assumes panda is up to date + with Timeout(10, "timed out waiting to go onroad"): + while not sm['deviceState'].started: + sm.update(100) + + # wait for engageability + try: + with Timeout(10, "timed out waiting for engageable"): + sendcan_frame = None + while True: + sm.update(100) + + # sendcan is only sent once we're initialized + if sm.seen['controlsState'] and sendcan_frame is None: + sendcan_frame = sm.frame + + if sendcan_frame is not None and sm.recv_frame['sendcan'] > sendcan_frame: + sm.update(100) + assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}" + break + finally: + print(f"onroad events: {sm['onroadEvents']}") + print(f"engageable after {time.monotonic() - start_time:.2f}s") + + # once we're enageable, must stay for the next few seconds + st = time.monotonic() + while (time.monotonic() - st) < 10.: + sm.update(100) + assert sm.all_alive(), sm.alive + assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}" + assert sm['controlsState'].cumLagMs < 10. + finally: + proc.terminate() + if proc.wait(20) is None: + proc.kill() diff --git a/selfdrive/test/test_updated.py b/selfdrive/test/test_updated.py new file mode 100644 index 0000000..dd79e03 --- /dev/null +++ b/selfdrive/test/test_updated.py @@ -0,0 +1,302 @@ +#!/usr/bin/env python3 +import datetime +import os +import pytest +import time +import tempfile +import unittest +import shutil +import signal +import subprocess +import random + +from openpilot.common.basedir import BASEDIR +from openpilot.common.params import Params + + +@pytest.mark.tici +class TestUpdated(unittest.TestCase): + + def setUp(self): + self.updated_proc = None + + self.tmp_dir = tempfile.TemporaryDirectory() + org_dir = os.path.join(self.tmp_dir.name, "commaai") + + self.basedir = os.path.join(org_dir, "openpilot") + self.git_remote_dir = os.path.join(org_dir, "openpilot_remote") + self.staging_dir = os.path.join(org_dir, "safe_staging") + for d in [org_dir, self.basedir, self.git_remote_dir, self.staging_dir]: + os.mkdir(d) + + self.neos_version = os.path.join(org_dir, "neos_version") + self.neosupdate_dir = os.path.join(org_dir, "neosupdate") + with open(self.neos_version, "w") as f: + v = subprocess.check_output(r"bash -c 'source launch_env.sh && echo $REQUIRED_NEOS_VERSION'", + cwd=BASEDIR, shell=True, encoding='utf8').strip() + f.write(v) + + self.upper_dir = os.path.join(self.staging_dir, "upper") + self.merged_dir = os.path.join(self.staging_dir, "merged") + self.finalized_dir = os.path.join(self.staging_dir, "finalized") + + # setup local submodule remotes + submodules = subprocess.check_output("git submodule --quiet foreach 'echo $name'", + shell=True, cwd=BASEDIR, encoding='utf8').split() + for s in submodules: + sub_path = os.path.join(org_dir, s.split("_repo")[0]) + self._run(f"git clone {s} {sub_path}.git", cwd=BASEDIR) + + # setup two git repos, a remote and one we'll run updated in + self._run([ + f"git clone {BASEDIR} {self.git_remote_dir}", + f"git clone {self.git_remote_dir} {self.basedir}", + f"cd {self.basedir} && git submodule init && git submodule update", + f"cd {self.basedir} && scons -j{os.cpu_count()} cereal/ common/" + ]) + + self.params = Params(os.path.join(self.basedir, "persist/params")) + self.params.clear_all() + os.sync() + + def tearDown(self): + try: + if self.updated_proc is not None: + self.updated_proc.terminate() + self.updated_proc.wait(30) + except Exception as e: + print(e) + self.tmp_dir.cleanup() + + + # *** test helpers *** + + + def _run(self, cmd, cwd=None): + if not isinstance(cmd, list): + cmd = (cmd,) + + for c in cmd: + subprocess.check_output(c, cwd=cwd, shell=True) + + def _get_updated_proc(self): + os.environ["PYTHONPATH"] = self.basedir + os.environ["GIT_AUTHOR_NAME"] = "testy tester" + os.environ["GIT_COMMITTER_NAME"] = "testy tester" + os.environ["GIT_AUTHOR_EMAIL"] = "testy@tester.test" + os.environ["GIT_COMMITTER_EMAIL"] = "testy@tester.test" + os.environ["UPDATER_TEST_IP"] = "localhost" + os.environ["UPDATER_LOCK_FILE"] = os.path.join(self.tmp_dir.name, "updater.lock") + os.environ["UPDATER_STAGING_ROOT"] = self.staging_dir + os.environ["UPDATER_NEOS_VERSION"] = self.neos_version + os.environ["UPDATER_NEOSUPDATE_DIR"] = self.neosupdate_dir + updated_path = os.path.join(self.basedir, "selfdrive/updated.py") + return subprocess.Popen(updated_path, env=os.environ) + + def _start_updater(self, offroad=True, nosleep=False): + self.params.put_bool("IsOffroad", offroad) + self.updated_proc = self._get_updated_proc() + if not nosleep: + time.sleep(1) + + def _update_now(self): + self.updated_proc.send_signal(signal.SIGHUP) + + # TODO: this should be implemented in params + def _read_param(self, key, timeout=1): + ret = None + start_time = time.monotonic() + while ret is None: + ret = self.params.get(key, encoding='utf8') + if time.monotonic() - start_time > timeout: + break + time.sleep(0.01) + return ret + + def _wait_for_update(self, timeout=30, clear_param=False): + if clear_param: + self.params.remove("LastUpdateTime") + + self._update_now() + t = self._read_param("LastUpdateTime", timeout=timeout) + if t is None: + raise Exception("timed out waiting for update to complete") + + def _make_commit(self): + all_dirs, all_files = [], [] + for root, dirs, files in os.walk(self.git_remote_dir): + if ".git" in root: + continue + for d in dirs: + all_dirs.append(os.path.join(root, d)) + for f in files: + all_files.append(os.path.join(root, f)) + + # make a new dir and some new files + new_dir = os.path.join(self.git_remote_dir, "this_is_a_new_dir") + os.mkdir(new_dir) + for _ in range(random.randrange(5, 30)): + for d in (new_dir, random.choice(all_dirs)): + with tempfile.NamedTemporaryFile(dir=d, delete=False) as f: + f.write(os.urandom(random.randrange(1, 1000000))) + + # modify some files + for f in random.sample(all_files, random.randrange(5, 50)): + with open(f, "w+") as ff: + txt = ff.readlines() + ff.seek(0) + for line in txt: + ff.write(line[::-1]) + + # remove some files + for f in random.sample(all_files, random.randrange(5, 50)): + os.remove(f) + + # remove some dirs + for d in random.sample(all_dirs, random.randrange(1, 10)): + shutil.rmtree(d) + + # commit the changes + self._run([ + "git add -A", + "git commit -m 'an update'", + ], cwd=self.git_remote_dir) + + def _check_update_state(self, update_available): + # make sure LastUpdateTime is recent + t = self._read_param("LastUpdateTime") + last_update_time = datetime.datetime.fromisoformat(t) + td = datetime.datetime.utcnow() - last_update_time + self.assertLess(td.total_seconds(), 10) + self.params.remove("LastUpdateTime") + + # wait a bit for the rest of the params to be written + time.sleep(0.1) + + # check params + update = self._read_param("UpdateAvailable") + self.assertEqual(update == "1", update_available, f"UpdateAvailable: {repr(update)}") + self.assertEqual(self._read_param("UpdateFailedCount"), "0") + + # TODO: check that the finalized update actually matches remote + # check the .overlay_init and .overlay_consistent flags + self.assertTrue(os.path.isfile(os.path.join(self.basedir, ".overlay_init"))) + self.assertEqual(os.path.isfile(os.path.join(self.finalized_dir, ".overlay_consistent")), update_available) + + + # *** test cases *** + + + # Run updated for 100 cycles with no update + def test_no_update(self): + self._start_updater() + for _ in range(100): + self._wait_for_update(clear_param=True) + self._check_update_state(False) + + # Let the updater run with no update for a cycle, then write an update + def test_update(self): + self._start_updater() + + # run for a cycle with no update + self._wait_for_update(clear_param=True) + self._check_update_state(False) + + # write an update to our remote + self._make_commit() + + # run for a cycle to get the update + self._wait_for_update(timeout=60, clear_param=True) + self._check_update_state(True) + + # run another cycle with no update + self._wait_for_update(clear_param=True) + self._check_update_state(True) + + # Let the updater run for 10 cycles, and write an update every cycle + @unittest.skip("need to make this faster") + def test_update_loop(self): + self._start_updater() + + # run for a cycle with no update + self._wait_for_update(clear_param=True) + for _ in range(10): + time.sleep(0.5) + self._make_commit() + self._wait_for_update(timeout=90, clear_param=True) + self._check_update_state(True) + + # Test overlay re-creation after tracking a new file in basedir's git + def test_overlay_reinit(self): + self._start_updater() + + overlay_init_fn = os.path.join(self.basedir, ".overlay_init") + + # run for a cycle with no update + self._wait_for_update(clear_param=True) + self.params.remove("LastUpdateTime") + first_mtime = os.path.getmtime(overlay_init_fn) + + # touch a file in the basedir + self._run("touch new_file && git add new_file", cwd=self.basedir) + + # run another cycle, should have a new mtime + self._wait_for_update(clear_param=True) + second_mtime = os.path.getmtime(overlay_init_fn) + self.assertTrue(first_mtime != second_mtime) + + # run another cycle, mtime should be same as last cycle + self._wait_for_update(clear_param=True) + new_mtime = os.path.getmtime(overlay_init_fn) + self.assertTrue(second_mtime == new_mtime) + + # Make sure updated exits if another instance is running + def test_multiple_instances(self): + # start updated and let it run for a cycle + self._start_updater() + time.sleep(1) + self._wait_for_update(clear_param=True) + + # start another instance + second_updated = self._get_updated_proc() + ret_code = second_updated.wait(timeout=5) + self.assertTrue(ret_code is not None) + + + # *** test cases with NEOS updates *** + + + # Run updated with no update, make sure it clears the old NEOS update + def test_clear_neos_cache(self): + # make the dir and some junk files + os.mkdir(self.neosupdate_dir) + for _ in range(15): + with tempfile.NamedTemporaryFile(dir=self.neosupdate_dir, delete=False) as f: + f.write(os.urandom(random.randrange(1, 1000000))) + + self._start_updater() + self._wait_for_update(clear_param=True) + self._check_update_state(False) + self.assertFalse(os.path.isdir(self.neosupdate_dir)) + + # Let the updater run with no update for a cycle, then write an update + @unittest.skip("TODO: only runs on device") + def test_update_with_neos_update(self): + # bump the NEOS version and commit it + self._run([ + "echo 'export REQUIRED_NEOS_VERSION=3' >> launch_env.sh", + "git -c user.name='testy' -c user.email='testy@tester.test' \ + commit -am 'a neos update'", + ], cwd=self.git_remote_dir) + + # run for a cycle to get the update + self._start_updater() + self._wait_for_update(timeout=60, clear_param=True) + self._check_update_state(True) + + # TODO: more comprehensive check + self.assertTrue(os.path.isdir(self.neosupdate_dir)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/test_valgrind_replay.py b/selfdrive/test/test_valgrind_replay.py new file mode 100644 index 0000000..75520df --- /dev/null +++ b/selfdrive/test/test_valgrind_replay.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +import os +import threading +import time +import unittest +import subprocess +import signal + +if "CI" in os.environ: + def tqdm(x): + return x +else: + from tqdm import tqdm # type: ignore + +import cereal.messaging as messaging +from collections import namedtuple +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.openpilotci import get_url +from openpilot.common.basedir import BASEDIR + +ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'command', 'path', 'segment', 'wait_for_response']) + +CONFIGS = [ + ProcessConfig( + proc_name="ubloxd", + pub_sub={ + "ubloxRaw": ["ubloxGnss", "gpsLocationExternal"], + }, + ignore=[], + command="./ubloxd", + path="system/ubloxd", + segment="0375fdf7b1ce594d|2019-06-13--08-32-25--3", + wait_for_response=True + ), +] + + +class TestValgrind(unittest.TestCase): + def extract_leak_sizes(self, log): + if "All heap blocks were freed -- no leaks are possible" in log: + return (0,0,0) + + log = log.replace(",","") # fixes casting to int issue with large leaks + err_lost1 = log.split("definitely lost: ")[1] + err_lost2 = log.split("indirectly lost: ")[1] + err_lost3 = log.split("possibly lost: ")[1] + definitely_lost = int(err_lost1.split(" ")[0]) + indirectly_lost = int(err_lost2.split(" ")[0]) + possibly_lost = int(err_lost3.split(" ")[0]) + return (definitely_lost, indirectly_lost, possibly_lost) + + def valgrindlauncher(self, arg, cwd): + os.chdir(os.path.join(BASEDIR, cwd)) + # Run valgrind on a process + command = "valgrind --leak-check=full " + arg + p = subprocess.Popen(command, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid) + + while not self.replay_done: + time.sleep(0.1) + + # Kill valgrind and extract leak output + os.killpg(os.getpgid(p.pid), signal.SIGINT) + _, err = p.communicate() + error_msg = str(err, encoding='utf-8') + with open(os.path.join(BASEDIR, "selfdrive/test/valgrind_logs.txt"), "a") as f: + f.write(error_msg) + f.write(5 * "\n") + definitely_lost, indirectly_lost, possibly_lost = self.extract_leak_sizes(error_msg) + if max(definitely_lost, indirectly_lost, possibly_lost) > 0: + self.leak = True + print("LEAKS from", arg, "\nDefinitely lost:", definitely_lost, "\nIndirectly lost", indirectly_lost, "\nPossibly lost", possibly_lost) + else: + self.leak = False + + def replay_process(self, config, logreader): + pub_sockets = list(config.pub_sub.keys()) # We dump data from logs here + sub_sockets = [s for _, sub in config.pub_sub.items() for s in sub] # We get responses here + pm = messaging.PubMaster(pub_sockets) + sm = messaging.SubMaster(sub_sockets) + + print("Sorting logs") + all_msgs = sorted(logreader, key=lambda msg: msg.logMonoTime) + pub_msgs = [msg for msg in all_msgs if msg.which() in list(config.pub_sub.keys())] + + thread = threading.Thread(target=self.valgrindlauncher, args=(config.command, config.path)) + thread.daemon = True + thread.start() + + while not all(pm.all_readers_updated(s) for s in config.pub_sub.keys()): + time.sleep(0) + + for msg in tqdm(pub_msgs): + pm.send(msg.which(), msg.as_builder()) + if config.wait_for_response: + sm.update(100) + + self.replay_done = True + + def test_config(self): + open(os.path.join(BASEDIR, "selfdrive/test/valgrind_logs.txt"), "w").close() + + for cfg in CONFIGS: + self.leak = None + self.replay_done = False + + r, n = cfg.segment.rsplit("--", 1) + lr = LogReader(get_url(r, n)) + self.replay_process(cfg, lr) + + while self.leak is None: + time.sleep(0.1) # Wait for the valgrind to finish + + self.assertFalse(self.leak) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py new file mode 100644 index 0000000..a9f4494 --- /dev/null +++ b/selfdrive/test/update_ci_routes.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +import os +import re +import subprocess +import sys +from collections.abc import Iterable + +from tqdm import tqdm + +from openpilot.selfdrive.car.tests.routes import routes as test_car_models_routes +from openpilot.selfdrive.test.process_replay.test_processes import source_segments as replay_segments +from openpilot.tools.lib.azure_container import AzureContainer +from openpilot.tools.lib.openpilotcontainers import DataCIContainer, DataProdContainer, OpenpilotCIContainer + +SOURCES: list[AzureContainer] = [ + DataProdContainer, + DataCIContainer +] + +DEST = OpenpilotCIContainer + +def upload_route(path: str, exclude_patterns: Iterable[str] = None) -> None: + if exclude_patterns is None: + exclude_patterns = [r'dcamera\.hevc'] + + r, n = path.rsplit("--", 1) + r = '/'.join(r.split('/')[-2:]) # strip out anything extra in the path + destpath = f"{r}/{n}" + for file in os.listdir(path): + if any(re.search(pattern, file) for pattern in exclude_patterns): + continue + DEST.upload_file(os.path.join(path, file), f"{destpath}/{file}") + + +def sync_to_ci_public(route: str) -> bool: + dest_container, dest_key = DEST.get_client_and_key() + key_prefix = route.replace('|', '/') + dongle_id = key_prefix.split('/')[0] + + if next(dest_container.list_blob_names(name_starts_with=key_prefix), None) is not None: + return True + + print(f"Uploading {route}") + for source_container in SOURCES: + # assumes az login has been run + print(f"Trying {source_container.ACCOUNT}/{source_container.CONTAINER}") + _, source_key = source_container.get_client_and_key() + cmd = [ + "azcopy", + "copy", + f"{source_container.BASE_URL}{key_prefix}?{source_key}", + f"{DEST.BASE_URL}{dongle_id}?{dest_key}", + "--recursive=true", + "--overwrite=false", + "--exclude-pattern=*/dcamera.hevc", + ] + + try: + result = subprocess.call(cmd, stdout=subprocess.DEVNULL) + if result == 0: + print("Success") + return True + except subprocess.CalledProcessError: + print("Failed") + + return False + + +if __name__ == "__main__": + failed_routes = [] + + to_sync = sys.argv[1:] + + if not len(to_sync): + # sync routes from the car tests routes and process replay + to_sync.extend([rt.route for rt in test_car_models_routes]) + to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments]) + + for r in tqdm(to_sync): + if not sync_to_ci_public(r): + failed_routes.append(r) + + if len(failed_routes): + print("failed routes:", failed_routes) diff --git a/selfdrive/thermald/__init__.py b/selfdrive/thermald/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 1378de5..7936f89 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -1,6 +1,5 @@ import time import threading -from typing import Optional from openpilot.common.params import Params from openpilot.system.hardware import HARDWARE @@ -38,12 +37,14 @@ class PowerMonitoring: self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) # FrogPilot variables - device_shutdown_setting = self.params.get_int("DeviceShutdown") + device_management = self.params.get_bool("DeviceManagement") + device_shutdown_setting = self.params.get_int("DeviceShutdown") if device_management else 33 # If the toggle is set for < 1 hour, configure by 15 minute increments self.device_shutdown_time = (device_shutdown_setting - 3) * 3600 if device_shutdown_setting >= 4 else device_shutdown_setting * (60 * 15) + self.low_voltage_shutdown = self.params.get_float("LowVoltageShutdown") if device_management else VBATT_PAUSE_CHARGING # Calculation tick - def calculate(self, voltage: Optional[int], ignition: bool): + def calculate(self, voltage: int | None, ignition: bool): try: now = time.monotonic() @@ -113,14 +114,14 @@ class PowerMonitoring: return int(self.car_battery_capacity_uWh) # See if we need to shutdown - def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: Optional[float], started_seen: bool): + def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: float | None, started_seen: bool): if offroad_timestamp is None: return False now = time.monotonic() should_shutdown = False offroad_time = (now - offroad_timestamp) - low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and + low_voltage_shutdown = (self.car_voltage_mV < (self.low_voltage_shutdown * 1e3) and offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S) should_shutdown |= offroad_time > self.device_shutdown_time should_shutdown |= low_voltage_shutdown @@ -128,7 +129,7 @@ class PowerMonitoring: should_shutdown &= not ignition should_shutdown &= (not self.params.get_bool("DisablePowerDown")) should_shutdown &= in_car - should_shutdown &= offroad_time > DELAY_SHUTDOWN_TIME_S if self.device_shutdown_time else True # If "Instant" is selected for the timer, shutdown immediately + should_shutdown &= offroad_time > DELAY_SHUTDOWN_TIME_S should_shutdown |= self.params.get_bool("ForcePowerDown") should_shutdown &= started_seen or (now > MIN_ON_TIME_S) return should_shutdown diff --git a/selfdrive/thermald/tests/__init__.py b/selfdrive/thermald/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/thermald/tests/test_fan_controller.py b/selfdrive/thermald/tests/test_fan_controller.py new file mode 100644 index 0000000..7081e13 --- /dev/null +++ b/selfdrive/thermald/tests/test_fan_controller.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 +import unittest +from unittest.mock import Mock, patch +from parameterized import parameterized + +from openpilot.selfdrive.thermald.fan_controller import TiciFanController + +ALL_CONTROLLERS = [(TiciFanController,)] + +def patched_controller(controller_class): + with patch("os.system", new=Mock()): + return controller_class() + +class TestFanController(unittest.TestCase): + def wind_up(self, controller, ignition=True): + for _ in range(1000): + controller.update(100, ignition) + + def wind_down(self, controller, ignition=False): + for _ in range(1000): + controller.update(10, ignition) + + @parameterized.expand(ALL_CONTROLLERS) + def test_hot_onroad(self, controller_class): + controller = patched_controller(controller_class) + self.wind_up(controller) + self.assertGreaterEqual(controller.update(100, True), 70) + + @parameterized.expand(ALL_CONTROLLERS) + def test_offroad_limits(self, controller_class): + controller = patched_controller(controller_class) + self.wind_up(controller) + self.assertLessEqual(controller.update(100, False), 30) + + @parameterized.expand(ALL_CONTROLLERS) + def test_no_fan_wear(self, controller_class): + controller = patched_controller(controller_class) + self.wind_down(controller) + self.assertEqual(controller.update(10, False), 0) + + @parameterized.expand(ALL_CONTROLLERS) + def test_limited(self, controller_class): + controller = patched_controller(controller_class) + self.wind_up(controller, True) + self.assertEqual(controller.update(100, True), 100) + + @parameterized.expand(ALL_CONTROLLERS) + def test_windup_speed(self, controller_class): + controller = patched_controller(controller_class) + self.wind_down(controller, True) + for _ in range(10): + controller.update(90, True) + self.assertGreaterEqual(controller.update(90, True), 60) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py new file mode 100644 index 0000000..c3a890f --- /dev/null +++ b/selfdrive/thermald/tests/test_power_monitoring.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3 +import unittest +from unittest.mock import patch + +from openpilot.common.params import Params +from openpilot.selfdrive.thermald.power_monitoring import PowerMonitoring, CAR_BATTERY_CAPACITY_uWh, \ + CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING, DELAY_SHUTDOWN_TIME_S + + +# Create fake time +ssb = 0. +def mock_time_monotonic(): + global ssb + ssb += 1. + return ssb + +TEST_DURATION_S = 50 +GOOD_VOLTAGE = 12 * 1e3 +VOLTAGE_BELOW_PAUSE_CHARGING = (VBATT_PAUSE_CHARGING - 1) * 1e3 + +def pm_patch(name, value, constant=False): + if constant: + return patch(f"openpilot.selfdrive.thermald.power_monitoring.{name}", value) + return patch(f"openpilot.selfdrive.thermald.power_monitoring.{name}", return_value=value) + + +@patch("time.monotonic", new=mock_time_monotonic) +class TestPowerMonitoring(unittest.TestCase): + def setUp(self): + self.params = Params() + + # Test to see that it doesn't do anything when pandaState is None + def test_pandaState_present(self): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(None, None) + self.assertEqual(pm.get_power_used(), 0) + self.assertEqual(pm.get_car_battery_capacity(), (CAR_BATTERY_CAPACITY_uWh / 10)) + + # Test to see that it doesn't integrate offroad when ignition is True + def test_offroad_ignition(self): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(GOOD_VOLTAGE, True) + self.assertEqual(pm.get_power_used(), 0) + + # Test to see that it integrates with discharging battery + def test_offroad_integration_discharging(self): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, False) + expected_power_usage = ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) + self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10) + + # Test to check positive integration of car_battery_capacity + def test_car_battery_integration_onroad(self): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 0 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, True) + expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) + + # Test to check positive integration upper limit + def test_car_battery_integration_upper_limit(self): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, True) + estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) + + # Test to check negative integration of car_battery_capacity + def test_car_battery_integration_offroad(self): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, False) + expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) + + # Test to check negative integration lower limit + def test_car_battery_integration_lower_limit(self): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, False) + estimated_capacity = 0 - ((1/3600) * POWER_DRAW * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) + + # Test to check policy of stopping charging after MAX_TIME_OFFROAD_S + def test_max_time_offroad(self): + MOCKED_MAX_OFFROAD_TIME = 3600 + POWER_DRAW = 0 # To stop shutting down for other reasons + with pm_patch("MAX_TIME_OFFROAD_S", MOCKED_MAX_OFFROAD_TIME, constant=True), pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + start_time = ssb + ignition = False + while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: + pm.calculate(GOOD_VOLTAGE, ignition) + if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: + self.assertFalse(pm.should_shutdown(ignition, True, start_time, False)) + self.assertTrue(pm.should_shutdown(ignition, True, start_time, False)) + + def test_car_voltage(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 350 + VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 50 + with pm_patch("VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S", VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S, constant=True), \ + pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = False + start_time = ssb + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + self.assertEqual(pm.should_shutdown(ignition, True, start_time, True), + (pm.car_voltage_mV < VBATT_PAUSE_CHARGING * 1e3 and + (ssb - start_time) > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S and + (ssb - start_time) > DELAY_SHUTDOWN_TIME_S)) + self.assertTrue(pm.should_shutdown(ignition, True, start_time, True)) + + # Test to check policy of not stopping charging when DisablePowerDown is set + def test_disable_power_down(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + self.params.put_bool("DisablePowerDown", True) + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = False + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + + # Test to check policy of not stopping charging when ignition + def test_ignition(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = True + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + + # Test to check policy of not stopping charging when harness is not connected + def test_harness_connection(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + + ignition = False + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + self.assertFalse(pm.should_shutdown(ignition, False, ssb, False)) + self.assertFalse(pm.should_shutdown(ignition, False, ssb, False)) + + def test_delay_shutdown_time(self): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 0 + ignition = False + in_car = True + offroad_timestamp = ssb + started_seen = True + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + + while ssb < offroad_timestamp + DELAY_SHUTDOWN_TIME_S: + self.assertFalse(pm.should_shutdown(ignition, in_car, + offroad_timestamp, + started_seen), + f"Should not shutdown before {DELAY_SHUTDOWN_TIME_S} seconds offroad time") + self.assertTrue(pm.should_shutdown(ignition, in_car, + offroad_timestamp, + started_seen), + f"Should shutdown after {DELAY_SHUTDOWN_TIME_S} seconds offroad time") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index ebd0a3e..926f0ce 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -6,7 +6,6 @@ import threading import time from collections import OrderedDict, namedtuple from pathlib import Path -from typing import Dict, Optional, Tuple import psutil @@ -50,9 +49,9 @@ THERMAL_BANDS = OrderedDict({ # Override to highest thermal band when offroad and above this temp OFFROAD_DANGER_TEMP = 75 -prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {} +prev_offroad_states: dict[str, tuple[bool, str | None]] = {} -tz_by_type: Optional[Dict[str, int]] = None +tz_by_type: dict[str, int] | None = None def populate_tz_by_type(): global tz_by_type tz_by_type = {} @@ -87,7 +86,7 @@ def read_thermal(thermal_config): return dat -def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: Optional[str]=None): +def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: str | None=None): if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text): return prev_offroad_states[offroad_alert] = (show_alert, extra_text) @@ -169,16 +168,16 @@ def thermald_thread(end_event, hw_queue) -> None: count = 0 - onroad_conditions: Dict[str, bool] = { + onroad_conditions: dict[str, bool] = { "ignition": False, } - startup_conditions: Dict[str, bool] = {} - startup_conditions_prev: Dict[str, bool] = {} + startup_conditions: dict[str, bool] = {} + startup_conditions_prev: dict[str, bool] = {} - off_ts: Optional[float] = None - started_ts: Optional[float] = None + off_ts: float | None = None + started_ts: float | None = None started_seen = False - startup_blocked_ts: Optional[float] = None + startup_blocked_ts: float | None = None thermal_status = ThermalStatus.yellow last_hw_state = HardwareState( @@ -205,6 +204,10 @@ def thermald_thread(end_event, hw_queue) -> None: fan_controller = None + # FrogPilot variables + device_management = params.get_bool("DeviceManagement") + offline_mode = device_management and params.get_bool("OfflineMode") + while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) @@ -217,6 +220,7 @@ def thermald_thread(end_event, hw_queue) -> None: peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown msg = read_thermal(thermal_config) + msg.deviceState.deviceType = HARDWARE.get_device_type() if sm.updated['pandaStates'] and len(pandaStates) > 0: @@ -297,12 +301,9 @@ def thermald_thread(end_event, hw_queue) -> None: elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] - if params.get_bool("FireTheBabysitter") and params.get_bool("MuteOverheated"): - thermal_status = ThermalStatus.green - # **** starting logic **** - startup_conditions["up_to_date"] = (params.get("OfflineMode") and params.get("FireTheBabysitter")) or params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") + startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") or offline_mode startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version diff --git a/selfdrive/ui_old.tgz b/selfdrive/ui_old.tgz deleted file mode 100644 index 815791c..0000000 Binary files a/selfdrive/ui_old.tgz and /dev/null differ diff --git a/selfdrive/updated/tests/test_base.py b/selfdrive/updated/tests/test_base.py new file mode 100644 index 0000000..1d81459 --- /dev/null +++ b/selfdrive/updated/tests/test_base.py @@ -0,0 +1,255 @@ +import os +import pathlib +import shutil +import signal +import stat +import subprocess +import tempfile +import time +import unittest +from unittest import mock + +import pytest +from openpilot.selfdrive.manager.process import ManagerProcess + + +from openpilot.selfdrive.test.helpers import processes_context +from openpilot.common.params import Params + + +def run(args, **kwargs): + return subprocess.run(args, **kwargs, check=True) + + +def update_release(directory, name, version, agnos_version, release_notes): + with open(directory / "RELEASES.md", "w") as f: + f.write(release_notes) + + (directory / "common").mkdir(exist_ok=True) + + with open(directory / "common" / "version.h", "w") as f: + f.write(f'#define COMMA_VERSION "{version}"') + + launch_env = directory / "launch_env.sh" + with open(launch_env, "w") as f: + f.write(f'export AGNOS_VERSION="{agnos_version}"') + + st = os.stat(launch_env) + os.chmod(launch_env, st.st_mode | stat.S_IEXEC) + + test_symlink = directory / "test_symlink" + if not os.path.exists(str(test_symlink)): + os.symlink("common/version.h", test_symlink) + + +def get_version(path: str) -> str: + with open(os.path.join(path, "common", "version.h")) as f: + return f.read().split('"')[1] + + +def get_consistent_flag(path: str) -> bool: + consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) + return consistent_file.is_file() + + +@pytest.mark.slow # TODO: can we test overlayfs in GHA? +class BaseUpdateTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + if "Base" in cls.__name__: + raise unittest.SkipTest + + def setUp(self): + self.tmpdir = tempfile.mkdtemp() + + run(["sudo", "mount", "-t", "tmpfs", "tmpfs", self.tmpdir]) # overlayfs doesn't work inside of docker unless this is a tmpfs + + self.mock_update_path = pathlib.Path(self.tmpdir) + + self.params = Params() + + self.basedir = self.mock_update_path / "openpilot" + self.basedir.mkdir() + + self.staging_root = self.mock_update_path / "safe_staging" + self.staging_root.mkdir() + + self.remote_dir = self.mock_update_path / "remote" + self.remote_dir.mkdir() + + mock.patch("openpilot.common.basedir.BASEDIR", self.basedir).start() + + os.environ["UPDATER_STAGING_ROOT"] = str(self.staging_root) + os.environ["UPDATER_LOCK_FILE"] = str(self.mock_update_path / "safe_staging_overlay.lock") + + self.MOCK_RELEASES = { + "release3": ("0.1.2", "1.2", "0.1.2 release notes"), + "master": ("0.1.3", "1.2", "0.1.3 release notes"), + } + + def set_target_branch(self, branch): + self.params.put("UpdaterTargetBranch", branch) + + def setup_basedir_release(self, release): + self.params = Params() + self.set_target_branch(release) + + def update_remote_release(self, release): + raise NotImplementedError("") + + def setup_remote_release(self, release): + raise NotImplementedError("") + + def additional_context(self): + raise NotImplementedError("") + + def tearDown(self): + mock.patch.stopall() + try: + run(["sudo", "umount", "-l", str(self.staging_root / "merged")]) + run(["sudo", "umount", "-l", self.tmpdir]) + shutil.rmtree(self.tmpdir) + except Exception: + print("cleanup failed...") + + def send_check_for_updates_signal(self, updated: ManagerProcess): + updated.signal(signal.SIGUSR1.value) + + def send_download_signal(self, updated: ManagerProcess): + updated.signal(signal.SIGHUP.value) + + def _test_params(self, branch, fetch_available, update_available): + self.assertEqual(self.params.get("UpdaterTargetBranch", encoding="utf-8"), branch) + self.assertEqual(self.params.get_bool("UpdaterFetchAvailable"), fetch_available) + self.assertEqual(self.params.get_bool("UpdateAvailable"), update_available) + + def _test_finalized_update(self, branch, version, agnos_version, release_notes): + self.assertTrue(self.params.get("UpdaterNewDescription", encoding="utf-8").startswith(f"{version} / {branch}")) + self.assertEqual(self.params.get("UpdaterNewReleaseNotes", encoding="utf-8"), f"

{release_notes}

\n") + self.assertEqual(get_version(str(self.staging_root / "finalized")), version) + self.assertEqual(get_consistent_flag(str(self.staging_root / "finalized")), True) + self.assertTrue(os.access(str(self.staging_root / "finalized" / "launch_env.sh"), os.X_OK)) + + with open(self.staging_root / "finalized" / "test_symlink") as f: + self.assertIn(version, f.read()) + + def wait_for_condition(self, condition, timeout=12): + start = time.monotonic() + while True: + waited = time.monotonic() - start + if condition(): + print(f"waited {waited}s for condition ") + return waited + + if waited > timeout: + raise TimeoutError("timed out waiting for condition") + + time.sleep(1) + + def wait_for_idle(self): + self.wait_for_condition(lambda: self.params.get("UpdaterState", encoding="utf-8") == "idle") + + def wait_for_fetch_available(self): + self.wait_for_condition(lambda: self.params.get_bool("UpdaterFetchAvailable")) + + def wait_for_update_available(self): + self.wait_for_condition(lambda: self.params.get_bool("UpdateAvailable")) + + def test_no_update(self): + # Start on release3, ensure we don't fetch any updates + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.send_check_for_updates_signal(updated) + + self.wait_for_idle() + + self._test_params("release3", False, False) + + def test_new_release(self): + # Start on release3, simulate a release3 commit, ensure we fetch that update properly + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.2", "0.1.3 release notes") + self.update_remote_release("release3") + + self.send_check_for_updates_signal(updated) + + self.wait_for_fetch_available() + + self._test_params("release3", True, False) + + self.send_download_signal(updated) + + self.wait_for_update_available() + + self._test_params("release3", False, True) + self._test_finalized_update("release3", *self.MOCK_RELEASES["release3"]) + + def test_switch_branches(self): + # Start on release3, request to switch to master manually, ensure we switched + self.setup_remote_release("release3") + self.setup_remote_release("master") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.set_target_branch("master") + self.send_check_for_updates_signal(updated) + + self.wait_for_fetch_available() + + self._test_params("master", True, False) + + self.send_download_signal(updated) + + self.wait_for_update_available() + + self._test_params("master", False, True) + self._test_finalized_update("master", *self.MOCK_RELEASES["master"]) + + def test_agnos_update(self): + # Start on release3, push an update with an agnos change + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), \ + mock.patch("openpilot.system.hardware.AGNOS", "True"), \ + mock.patch("openpilot.system.hardware.tici.hardware.Tici.get_os_version", "1.2"), \ + mock.patch("openpilot.system.hardware.tici.agnos.get_target_slot_number"), \ + mock.patch("openpilot.system.hardware.tici.agnos.flash_agnos_update"), \ + processes_context(["updated"]) as [updated]: + + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.3", "0.1.3 release notes") + self.update_remote_release("release3") + + self.send_check_for_updates_signal(updated) + + self.wait_for_fetch_available() + + self._test_params("release3", True, False) + + self.send_download_signal(updated) + + self.wait_for_update_available() + + self._test_params("release3", False, True) + self._test_finalized_update("release3", *self.MOCK_RELEASES["release3"]) diff --git a/selfdrive/updated/tests/test_git.py b/selfdrive/updated/tests/test_git.py new file mode 100644 index 0000000..1a9c782 --- /dev/null +++ b/selfdrive/updated/tests/test_git.py @@ -0,0 +1,22 @@ +import contextlib +from openpilot.selfdrive.updated.tests.test_base import BaseUpdateTest, run, update_release + + +class TestUpdateDGitStrategy(BaseUpdateTest): + def update_remote_release(self, release): + update_release(self.remote_dir, release, *self.MOCK_RELEASES[release]) + run(["git", "add", "."], cwd=self.remote_dir) + run(["git", "commit", "-m", f"openpilot release {release}"], cwd=self.remote_dir) + + def setup_remote_release(self, release): + run(["git", "init"], cwd=self.remote_dir) + run(["git", "checkout", "-b", release], cwd=self.remote_dir) + self.update_remote_release(release) + + def setup_basedir_release(self, release): + super().setup_basedir_release(release) + run(["git", "clone", "-b", release, self.remote_dir, self.basedir]) + + @contextlib.contextmanager + def additional_context(self): + yield diff --git a/selfdrive/updated.py b/selfdrive/updated/updated.py similarity index 89% rename from selfdrive/updated.py rename to selfdrive/updated/updated.py index f63c71b..9084e20 100644 --- a/selfdrive/updated.py +++ b/selfdrive/updated/updated.py @@ -11,7 +11,6 @@ import time import threading from collections import defaultdict from pathlib import Path -from typing import List, Union, Optional from markdown_it import MarkdownIt from zoneinfo import ZoneInfo @@ -65,7 +64,7 @@ def write_time_to_param(params, param) -> None: t = datetime.datetime.utcnow() params.put(param, t.isoformat().encode('utf8')) -def read_time_from_param(params, param) -> Optional[datetime.datetime]: +def read_time_from_param(params, param) -> datetime.datetime | None: t = params.get(param, encoding='utf8') try: return datetime.datetime.fromisoformat(t) @@ -73,7 +72,7 @@ def read_time_from_param(params, param) -> Optional[datetime.datetime]: pass return None -def run(cmd: List[str], cwd: Optional[str] = None) -> str: +def run(cmd: list[str], cwd: str = None) -> str: return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') @@ -235,11 +234,11 @@ def handle_agnos_update() -> None: class Updater: def __init__(self): self.params = Params() - self.branches = defaultdict(lambda: '') + self.branches = defaultdict(str) self._has_internet: bool = False # FrogPilot variables - self.disable_internet_check = self.params.get_bool("OfflineMode") and self.params.get_bool("FireTheBabysitter") + self.offline_mode = self.params.get_bool("DeviceManagement") and self.params.get_bool("OfflineMode") @property def has_internet(self) -> bool: @@ -247,7 +246,7 @@ class Updater: @property def target_branch(self) -> str: - b: Union[str, None] = self.params.get("UpdaterTargetBranch", encoding='utf-8') + b: str | None = self.params.get("UpdaterTargetBranch", encoding='utf-8') if b is None: b = self.get_branch(BASEDIR) return b @@ -276,7 +275,7 @@ class Updater: def get_commit_hash(self, path: str = OVERLAY_MERGED) -> str: return run(["git", "rev-parse", "HEAD"], path).rstrip() - def set_params(self, update_success: bool, failed_count: int, exception: Optional[str]) -> None: + def set_params(self, update_success: bool, failed_count: int, exception: str | None) -> None: self.params.put("UpdateFailedCount", str(failed_count)) self.params.put("UpdaterTargetBranch", self.target_branch) @@ -329,7 +328,7 @@ class Updater: set_offroad_alert(alert, False) now = datetime.datetime.utcnow() - if self.disable_internet_check: + if self.offline_mode: last_update = now dt = now - last_update if failed_count > 15 and exception is not None and self.has_internet: @@ -411,10 +410,12 @@ class Updater: finalize_update() cloudlog.info("finalize success!") + # Format "Updated" to Phoenix time zone self.params.put("Updated", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) def main() -> None: params = Params() + params_memory = Params("/dev/shm/params") if params.get_bool("DisableUpdates"): cloudlog.warning("updates are disabled by the DisableUpdates param") @@ -435,10 +436,6 @@ def main() -> None: if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir(): cloudlog.event("update installed") - # Format InstallDate to Phoenix time zone with full date-time - if params.get("InstallDate") is None or params.get("Updated") is None: - params.put("InstallDate", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) - updater = Updater() update_failed_count = 0 # TODO: Load from param? wait_helper = WaitTimeHelper() @@ -451,6 +448,10 @@ def main() -> None: # Run the update loop first_run = True + branches_set = "FrogPilot" in (params.get("UpdaterAvailableBranches", encoding='utf-8') or "").split(',') + install_date_set = params.get("InstallDate") is not None and params.get("Updated") is not None + install_date_set &= params.get("InstallDate") != "November 21, 2023 - 02:10PM" # Remove this on the June 1st update + while True: wait_helper.ready_event.clear() @@ -468,25 +469,35 @@ def main() -> None: wait_helper.sleep(60) continue + # Format "InstallDate" to Phoenix time zone + if not install_date_set: + params.put("InstallDate", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) + install_date_set = True + + if not (params.get_bool("AutomaticUpdates") or params_memory.get_bool("ManualUpdateInitiated") or not branches_set): + wait_helper.sleep(60*60*24*365*100) + continue + update_failed_count += 1 # check for update - if params.get_int("UpdateSchedule") != 0 or params.get_bool("ManualUpdateInitiated"): - params.put("UpdaterState", "checking...") - updater.check_for_update() + params.put("UpdaterState", "checking...") + updater.check_for_update() + branches_set = True + params_memory.put_bool("ManualUpdateInitiated", False) - # download update - last_fetch = read_time_from_param(params, "UpdaterLastFetchTime") - timed_out = last_fetch is None or (datetime.datetime.utcnow() - last_fetch > datetime.timedelta(days=3)) - user_requested_fetch = wait_helper.user_request == UserRequest.FETCH - if params.get_bool("NetworkMetered") and not timed_out and not user_requested_fetch: - cloudlog.info("skipping fetch, connection metered") - elif wait_helper.user_request == UserRequest.CHECK: - cloudlog.info("skipping fetch, only checking") - else: - updater.fetch_update() - write_time_to_param(params, "UpdaterLastFetchTime") - update_failed_count = 0 + # download update + last_fetch = read_time_from_param(params, "UpdaterLastFetchTime") + timed_out = last_fetch is None or (datetime.datetime.utcnow() - last_fetch > datetime.timedelta(days=3)) + user_requested_fetch = wait_helper.user_request == UserRequest.FETCH + if params.get_bool("NetworkMetered") and not timed_out and not user_requested_fetch: + cloudlog.info("skipping fetch, connection metered") + elif wait_helper.user_request == UserRequest.CHECK: + cloudlog.info("skipping fetch, only checking") + else: + updater.fetch_update() + write_time_to_param(params, "UpdaterLastFetchTime") + update_failed_count = 0 except subprocess.CalledProcessError as e: cloudlog.event( "update process failed", diff --git a/sfa/custom_themes/frog_theme/images/button_flag.png b/sfa/custom_themes/frog_theme/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/button_flag.png differ diff --git a/sfa/custom_themes/frog_theme/images/button_home.png b/sfa/custom_themes/frog_theme/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/button_home.png differ diff --git a/sfa/custom_themes/frog_theme/images/button_settings.png b/sfa/custom_themes/frog_theme/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/button_settings.png differ diff --git a/sfa/custom_themes/frog_theme/images/turn_signal_1.png b/sfa/custom_themes/frog_theme/images/turn_signal_1.png new file mode 100644 index 0000000..43e0b44 Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/turn_signal_1.png differ diff --git a/sfa/custom_themes/frog_theme/images/turn_signal_1_red.png b/sfa/custom_themes/frog_theme/images/turn_signal_1_red.png new file mode 100644 index 0000000..7c10245 Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/turn_signal_1_red.png differ diff --git a/sfa/custom_themes/frog_theme/images/turn_signal_2.png b/sfa/custom_themes/frog_theme/images/turn_signal_2.png new file mode 100644 index 0000000..e8e1479 Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/turn_signal_2.png differ diff --git a/sfa/custom_themes/frog_theme/images/turn_signal_3.png b/sfa/custom_themes/frog_theme/images/turn_signal_3.png new file mode 100644 index 0000000..b59b003 Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/turn_signal_3.png differ diff --git a/sfa/custom_themes/frog_theme/images/turn_signal_4.png b/sfa/custom_themes/frog_theme/images/turn_signal_4.png new file mode 100644 index 0000000..c3c1d20 Binary files /dev/null and b/sfa/custom_themes/frog_theme/images/turn_signal_4.png differ diff --git a/sfa/custom_themes/frog_theme/sounds/disengage.wav b/sfa/custom_themes/frog_theme/sounds/disengage.wav new file mode 100644 index 0000000..d46dd9e Binary files /dev/null and b/sfa/custom_themes/frog_theme/sounds/disengage.wav differ diff --git a/sfa/custom_themes/frog_theme/sounds/engage.wav b/sfa/custom_themes/frog_theme/sounds/engage.wav new file mode 100644 index 0000000..795aa53 Binary files /dev/null and b/sfa/custom_themes/frog_theme/sounds/engage.wav differ diff --git a/selfdrive/assets/sounds/goat.wav b/sfa/custom_themes/frog_theme/sounds/goat.wav similarity index 100% rename from selfdrive/assets/sounds/goat.wav rename to sfa/custom_themes/frog_theme/sounds/goat.wav diff --git a/sfa/custom_themes/stalin_theme/images/button_flag.png b/sfa/custom_themes/stalin_theme/images/button_flag.png new file mode 100644 index 0000000..1dca7ef Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/button_flag.png differ diff --git a/sfa/custom_themes/stalin_theme/images/button_home.png b/sfa/custom_themes/stalin_theme/images/button_home.png new file mode 100644 index 0000000..1dca7ef Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/button_home.png differ diff --git a/sfa/custom_themes/stalin_theme/images/button_settings.png b/sfa/custom_themes/stalin_theme/images/button_settings.png new file mode 100644 index 0000000..b58a726 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/button_settings.png differ diff --git a/sfa/custom_themes/stalin_theme/images/turn_signal_1.png b/sfa/custom_themes/stalin_theme/images/turn_signal_1.png new file mode 100644 index 0000000..f5116df Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/turn_signal_1.png differ diff --git a/sfa/custom_themes/stalin_theme/images/turn_signal_1_red.png b/sfa/custom_themes/stalin_theme/images/turn_signal_1_red.png new file mode 100644 index 0000000..4c94813 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/turn_signal_1_red.png differ diff --git a/sfa/custom_themes/stalin_theme/images/turn_signal_2.png b/sfa/custom_themes/stalin_theme/images/turn_signal_2.png new file mode 100644 index 0000000..8ceed23 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/turn_signal_2.png differ diff --git a/sfa/custom_themes/stalin_theme/images/turn_signal_3.png b/sfa/custom_themes/stalin_theme/images/turn_signal_3.png new file mode 100644 index 0000000..48dbe09 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/turn_signal_3.png differ diff --git a/sfa/custom_themes/stalin_theme/images/turn_signal_4.png b/sfa/custom_themes/stalin_theme/images/turn_signal_4.png new file mode 100644 index 0000000..3e46279 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/images/turn_signal_4.png differ diff --git a/sfa/custom_themes/stalin_theme/sounds/disengage.wav b/sfa/custom_themes/stalin_theme/sounds/disengage.wav new file mode 100644 index 0000000..4cb1586 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/sounds/disengage.wav differ diff --git a/sfa/custom_themes/stalin_theme/sounds/engage.wav b/sfa/custom_themes/stalin_theme/sounds/engage.wav new file mode 100644 index 0000000..4e5b747 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/sounds/engage.wav differ diff --git a/sfa/custom_themes/stalin_theme/sounds/refuse.wav b/sfa/custom_themes/stalin_theme/sounds/refuse.wav new file mode 100644 index 0000000..ef69b98 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/sounds/refuse.wav differ diff --git a/sfa/custom_themes/stalin_theme/sounds/warning_soft.wav b/sfa/custom_themes/stalin_theme/sounds/warning_soft.wav new file mode 100644 index 0000000..ee9b711 Binary files /dev/null and b/sfa/custom_themes/stalin_theme/sounds/warning_soft.wav differ diff --git a/sfa/custom_themes/tesla_theme/images/button_flag.png b/sfa/custom_themes/tesla_theme/images/button_flag.png new file mode 100644 index 0000000..2a140b9 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/button_flag.png differ diff --git a/sfa/custom_themes/tesla_theme/images/button_home.png b/sfa/custom_themes/tesla_theme/images/button_home.png new file mode 100644 index 0000000..2a140b9 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/button_home.png differ diff --git a/sfa/custom_themes/tesla_theme/images/button_settings.png b/sfa/custom_themes/tesla_theme/images/button_settings.png new file mode 100644 index 0000000..549e2be Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/button_settings.png differ diff --git a/sfa/custom_themes/tesla_theme/images/turn_signal_1.png b/sfa/custom_themes/tesla_theme/images/turn_signal_1.png new file mode 100644 index 0000000..b635c57 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/turn_signal_1.png differ diff --git a/sfa/custom_themes/tesla_theme/images/turn_signal_1_red.png b/sfa/custom_themes/tesla_theme/images/turn_signal_1_red.png new file mode 100644 index 0000000..a2f508f Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/turn_signal_1_red.png differ diff --git a/sfa/custom_themes/tesla_theme/images/turn_signal_2.png b/sfa/custom_themes/tesla_theme/images/turn_signal_2.png new file mode 100644 index 0000000..5158c0f Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/turn_signal_2.png differ diff --git a/sfa/custom_themes/tesla_theme/images/turn_signal_3.png b/sfa/custom_themes/tesla_theme/images/turn_signal_3.png new file mode 100644 index 0000000..c0e2a87 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/turn_signal_3.png differ diff --git a/sfa/custom_themes/tesla_theme/images/turn_signal_4.png b/sfa/custom_themes/tesla_theme/images/turn_signal_4.png new file mode 100644 index 0000000..5932e83 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/images/turn_signal_4.png differ diff --git a/sfa/custom_themes/tesla_theme/sounds/disengage.wav b/sfa/custom_themes/tesla_theme/sounds/disengage.wav new file mode 100644 index 0000000..81dcc4b Binary files /dev/null and b/sfa/custom_themes/tesla_theme/sounds/disengage.wav differ diff --git a/sfa/custom_themes/tesla_theme/sounds/engage.wav b/sfa/custom_themes/tesla_theme/sounds/engage.wav new file mode 100644 index 0000000..108f7cf Binary files /dev/null and b/sfa/custom_themes/tesla_theme/sounds/engage.wav differ diff --git a/sfa/custom_themes/tesla_theme/sounds/prompt_distracted.wav b/sfa/custom_themes/tesla_theme/sounds/prompt_distracted.wav new file mode 100644 index 0000000..c8b6132 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/sounds/prompt_distracted.wav differ diff --git a/sfa/custom_themes/tesla_theme/sounds/warning_immediate.wav b/sfa/custom_themes/tesla_theme/sounds/warning_immediate.wav new file mode 100644 index 0000000..f038f13 Binary files /dev/null and b/sfa/custom_themes/tesla_theme/sounds/warning_immediate.wav differ diff --git a/sfa/custom_themes/tesla_theme/sounds/warning_soft.wav b/sfa/custom_themes/tesla_theme/sounds/warning_soft.wav new file mode 100644 index 0000000..d02180b Binary files /dev/null and b/sfa/custom_themes/tesla_theme/sounds/warning_soft.wav differ diff --git a/sfa/holiday_themes/april_fools/images/button_flag.png b/sfa/holiday_themes/april_fools/images/button_flag.png new file mode 100644 index 0000000..758a824 Binary files /dev/null and b/sfa/holiday_themes/april_fools/images/button_flag.png differ diff --git a/sfa/holiday_themes/april_fools/images/button_home.png b/sfa/holiday_themes/april_fools/images/button_home.png new file mode 100644 index 0000000..6337bd4 Binary files /dev/null and b/sfa/holiday_themes/april_fools/images/button_home.png differ diff --git a/sfa/holiday_themes/april_fools/images/button_settings.png b/sfa/holiday_themes/april_fools/images/button_settings.png new file mode 100644 index 0000000..65ddf3f Binary files /dev/null and b/sfa/holiday_themes/april_fools/images/button_settings.png differ diff --git a/sfa/holiday_themes/april_fools/sounds/disengage.wav b/sfa/holiday_themes/april_fools/sounds/disengage.wav new file mode 100644 index 0000000..362bb11 Binary files /dev/null and b/sfa/holiday_themes/april_fools/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/april_fools/sounds/engage.wav b/sfa/holiday_themes/april_fools/sounds/engage.wav new file mode 100644 index 0000000..f40e839 Binary files /dev/null and b/sfa/holiday_themes/april_fools/sounds/engage.wav differ diff --git a/sfa/holiday_themes/april_fools/sounds/prompt.wav b/sfa/holiday_themes/april_fools/sounds/prompt.wav new file mode 100644 index 0000000..4b4ce41 Binary files /dev/null and b/sfa/holiday_themes/april_fools/sounds/prompt.wav differ diff --git a/sfa/holiday_themes/christmas/images/button_flag.png b/sfa/holiday_themes/christmas/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/christmas/images/button_flag.png differ diff --git a/sfa/holiday_themes/christmas/images/button_home.png b/sfa/holiday_themes/christmas/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/christmas/images/button_home.png differ diff --git a/sfa/holiday_themes/christmas/images/button_settings.png b/sfa/holiday_themes/christmas/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/christmas/images/button_settings.png differ diff --git a/sfa/holiday_themes/christmas/sounds/disengage.wav b/sfa/holiday_themes/christmas/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/christmas/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/christmas/sounds/engage.wav b/sfa/holiday_themes/christmas/sounds/engage.wav new file mode 100644 index 0000000..41e9b2d Binary files /dev/null and b/sfa/holiday_themes/christmas/sounds/engage.wav differ diff --git a/sfa/holiday_themes/cinco_de_mayo/images/button_flag.png b/sfa/holiday_themes/cinco_de_mayo/images/button_flag.png new file mode 100644 index 0000000..dd07122 Binary files /dev/null and b/sfa/holiday_themes/cinco_de_mayo/images/button_flag.png differ diff --git a/sfa/holiday_themes/cinco_de_mayo/images/button_home.png b/sfa/holiday_themes/cinco_de_mayo/images/button_home.png new file mode 100644 index 0000000..dd07122 Binary files /dev/null and b/sfa/holiday_themes/cinco_de_mayo/images/button_home.png differ diff --git a/sfa/holiday_themes/cinco_de_mayo/images/button_settings.png b/sfa/holiday_themes/cinco_de_mayo/images/button_settings.png new file mode 100644 index 0000000..70c4e04 Binary files /dev/null and b/sfa/holiday_themes/cinco_de_mayo/images/button_settings.png differ diff --git a/sfa/holiday_themes/cinco_de_mayo/sounds/disengage.wav b/sfa/holiday_themes/cinco_de_mayo/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/cinco_de_mayo/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/cinco_de_mayo/sounds/engage.wav b/sfa/holiday_themes/cinco_de_mayo/sounds/engage.wav new file mode 100644 index 0000000..ed0c8a6 Binary files /dev/null and b/sfa/holiday_themes/cinco_de_mayo/sounds/engage.wav differ diff --git a/sfa/holiday_themes/easter/images/button_flag.png b/sfa/holiday_themes/easter/images/button_flag.png new file mode 100644 index 0000000..6f8b178 Binary files /dev/null and b/sfa/holiday_themes/easter/images/button_flag.png differ diff --git a/sfa/holiday_themes/easter/images/button_home.png b/sfa/holiday_themes/easter/images/button_home.png new file mode 100644 index 0000000..6f8b178 Binary files /dev/null and b/sfa/holiday_themes/easter/images/button_home.png differ diff --git a/sfa/holiday_themes/easter/images/button_settings.png b/sfa/holiday_themes/easter/images/button_settings.png new file mode 100644 index 0000000..9a74f94 Binary files /dev/null and b/sfa/holiday_themes/easter/images/button_settings.png differ diff --git a/sfa/holiday_themes/easter/images/turn_signal_1.png b/sfa/holiday_themes/easter/images/turn_signal_1.png new file mode 100644 index 0000000..ea18185 Binary files /dev/null and b/sfa/holiday_themes/easter/images/turn_signal_1.png differ diff --git a/sfa/holiday_themes/easter/images/turn_signal_1_red.png b/sfa/holiday_themes/easter/images/turn_signal_1_red.png new file mode 100644 index 0000000..748ea30 Binary files /dev/null and b/sfa/holiday_themes/easter/images/turn_signal_1_red.png differ diff --git a/sfa/holiday_themes/easter/images/turn_signal_2.png b/sfa/holiday_themes/easter/images/turn_signal_2.png new file mode 100644 index 0000000..4331e5f Binary files /dev/null and b/sfa/holiday_themes/easter/images/turn_signal_2.png differ diff --git a/sfa/holiday_themes/easter/images/turn_signal_3.png b/sfa/holiday_themes/easter/images/turn_signal_3.png new file mode 100644 index 0000000..1a7484f Binary files /dev/null and b/sfa/holiday_themes/easter/images/turn_signal_3.png differ diff --git a/sfa/holiday_themes/easter/images/turn_signal_4.png b/sfa/holiday_themes/easter/images/turn_signal_4.png new file mode 100644 index 0000000..7514f2f Binary files /dev/null and b/sfa/holiday_themes/easter/images/turn_signal_4.png differ diff --git a/sfa/holiday_themes/easter/sounds/disengage.wav b/sfa/holiday_themes/easter/sounds/disengage.wav new file mode 100644 index 0000000..f56a0f8 Binary files /dev/null and b/sfa/holiday_themes/easter/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/easter/sounds/engage.wav b/sfa/holiday_themes/easter/sounds/engage.wav new file mode 100644 index 0000000..a8dd79d Binary files /dev/null and b/sfa/holiday_themes/easter/sounds/engage.wav differ diff --git a/sfa/holiday_themes/fourth_of_july/images/button_flag.png b/sfa/holiday_themes/fourth_of_july/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/fourth_of_july/images/button_flag.png differ diff --git a/sfa/holiday_themes/fourth_of_july/images/button_home.png b/sfa/holiday_themes/fourth_of_july/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/fourth_of_july/images/button_home.png differ diff --git a/sfa/holiday_themes/fourth_of_july/images/button_settings.png b/sfa/holiday_themes/fourth_of_july/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/fourth_of_july/images/button_settings.png differ diff --git a/sfa/holiday_themes/fourth_of_july/sounds/disengage.wav b/sfa/holiday_themes/fourth_of_july/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/fourth_of_july/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/fourth_of_july/sounds/engage.wav b/sfa/holiday_themes/fourth_of_july/sounds/engage.wav new file mode 100644 index 0000000..41e9b2d Binary files /dev/null and b/sfa/holiday_themes/fourth_of_july/sounds/engage.wav differ diff --git a/sfa/holiday_themes/halloween/images/button_flag.png b/sfa/holiday_themes/halloween/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/halloween/images/button_flag.png differ diff --git a/sfa/holiday_themes/halloween/images/button_home.png b/sfa/holiday_themes/halloween/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/halloween/images/button_home.png differ diff --git a/sfa/holiday_themes/halloween/images/button_settings.png b/sfa/holiday_themes/halloween/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/halloween/images/button_settings.png differ diff --git a/sfa/holiday_themes/halloween/sounds/disengage.wav b/sfa/holiday_themes/halloween/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/halloween/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/halloween/sounds/engage.wav b/sfa/holiday_themes/halloween/sounds/engage.wav new file mode 100644 index 0000000..41e9b2d Binary files /dev/null and b/sfa/holiday_themes/halloween/sounds/engage.wav differ diff --git a/sfa/holiday_themes/new_years_day/images/button_flag.png b/sfa/holiday_themes/new_years_day/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/new_years_day/images/button_flag.png differ diff --git a/sfa/holiday_themes/new_years_day/images/button_home.png b/sfa/holiday_themes/new_years_day/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/new_years_day/images/button_home.png differ diff --git a/sfa/holiday_themes/new_years_day/images/button_settings.png b/sfa/holiday_themes/new_years_day/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/new_years_day/images/button_settings.png differ diff --git a/sfa/holiday_themes/new_years_day/sounds/disengage.wav b/sfa/holiday_themes/new_years_day/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/new_years_day/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/new_years_day/sounds/engage.wav b/sfa/holiday_themes/new_years_day/sounds/engage.wav new file mode 100644 index 0000000..41e9b2d Binary files /dev/null and b/sfa/holiday_themes/new_years_day/sounds/engage.wav differ diff --git a/sfa/holiday_themes/st_patricks_day/images/button_flag.png b/sfa/holiday_themes/st_patricks_day/images/button_flag.png new file mode 100644 index 0000000..ada686a Binary files /dev/null and b/sfa/holiday_themes/st_patricks_day/images/button_flag.png differ diff --git a/sfa/holiday_themes/st_patricks_day/images/button_home.png b/sfa/holiday_themes/st_patricks_day/images/button_home.png new file mode 100644 index 0000000..ada686a Binary files /dev/null and b/sfa/holiday_themes/st_patricks_day/images/button_home.png differ diff --git a/sfa/holiday_themes/st_patricks_day/images/button_settings.png b/sfa/holiday_themes/st_patricks_day/images/button_settings.png new file mode 100644 index 0000000..7148616 Binary files /dev/null and b/sfa/holiday_themes/st_patricks_day/images/button_settings.png differ diff --git a/sfa/holiday_themes/st_patricks_day/sounds/disengage.wav b/sfa/holiday_themes/st_patricks_day/sounds/disengage.wav new file mode 100644 index 0000000..6f95b6f Binary files /dev/null and b/sfa/holiday_themes/st_patricks_day/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/st_patricks_day/sounds/engage.wav b/sfa/holiday_themes/st_patricks_day/sounds/engage.wav new file mode 100644 index 0000000..fc13a3a Binary files /dev/null and b/sfa/holiday_themes/st_patricks_day/sounds/engage.wav differ diff --git a/sfa/holiday_themes/thanksgiving/images/button_flag.png b/sfa/holiday_themes/thanksgiving/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/images/button_flag.png differ diff --git a/sfa/holiday_themes/thanksgiving/images/button_home.png b/sfa/holiday_themes/thanksgiving/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/images/button_home.png differ diff --git a/sfa/holiday_themes/thanksgiving/images/button_settings.png b/sfa/holiday_themes/thanksgiving/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/images/button_settings.png differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/disengage.wav b/sfa/holiday_themes/thanksgiving/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/engage.wav b/sfa/holiday_themes/thanksgiving/sounds/engage.wav new file mode 100644 index 0000000..41e9b2d Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/engage.wav differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/prompt.wav b/sfa/holiday_themes/thanksgiving/sounds/prompt.wav new file mode 100644 index 0000000..1ae7705 Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/prompt.wav differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/prompt_distracted.wav b/sfa/holiday_themes/thanksgiving/sounds/prompt_distracted.wav new file mode 100644 index 0000000..c3d4475 Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/prompt_distracted.wav differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/refuse.wav b/sfa/holiday_themes/thanksgiving/sounds/refuse.wav new file mode 100644 index 0000000..0e80f7d Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/refuse.wav differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/warning_immediate.wav b/sfa/holiday_themes/thanksgiving/sounds/warning_immediate.wav new file mode 100644 index 0000000..b1815a9 Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/warning_immediate.wav differ diff --git a/sfa/holiday_themes/thanksgiving/sounds/warning_soft.wav b/sfa/holiday_themes/thanksgiving/sounds/warning_soft.wav new file mode 100644 index 0000000..261c7e1 Binary files /dev/null and b/sfa/holiday_themes/thanksgiving/sounds/warning_soft.wav differ diff --git a/sfa/holiday_themes/valentines_day/images/button_flag.png b/sfa/holiday_themes/valentines_day/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/valentines_day/images/button_flag.png differ diff --git a/sfa/holiday_themes/valentines_day/images/button_home.png b/sfa/holiday_themes/valentines_day/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/valentines_day/images/button_home.png differ diff --git a/sfa/holiday_themes/valentines_day/images/button_settings.png b/sfa/holiday_themes/valentines_day/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/valentines_day/images/button_settings.png differ diff --git a/sfa/holiday_themes/valentines_day/sounds/disengage.wav b/sfa/holiday_themes/valentines_day/sounds/disengage.wav new file mode 100644 index 0000000..ba583c4 Binary files /dev/null and b/sfa/holiday_themes/valentines_day/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/valentines_day/sounds/engage.wav b/sfa/holiday_themes/valentines_day/sounds/engage.wav new file mode 100644 index 0000000..41e9b2d Binary files /dev/null and b/sfa/holiday_themes/valentines_day/sounds/engage.wav differ diff --git a/sfa/holiday_themes/world_frog_day/images/button_flag.png b/sfa/holiday_themes/world_frog_day/images/button_flag.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/button_flag.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/button_home.png b/sfa/holiday_themes/world_frog_day/images/button_home.png new file mode 100644 index 0000000..627af8a Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/button_home.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/button_settings.png b/sfa/holiday_themes/world_frog_day/images/button_settings.png new file mode 100644 index 0000000..5592759 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/button_settings.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/turn_signal_1.png b/sfa/holiday_themes/world_frog_day/images/turn_signal_1.png new file mode 100644 index 0000000..43e0b44 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/turn_signal_1.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/turn_signal_1_red.png b/sfa/holiday_themes/world_frog_day/images/turn_signal_1_red.png new file mode 100644 index 0000000..7c10245 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/turn_signal_1_red.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/turn_signal_2.png b/sfa/holiday_themes/world_frog_day/images/turn_signal_2.png new file mode 100644 index 0000000..e8e1479 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/turn_signal_2.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/turn_signal_3.png b/sfa/holiday_themes/world_frog_day/images/turn_signal_3.png new file mode 100644 index 0000000..b59b003 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/turn_signal_3.png differ diff --git a/sfa/holiday_themes/world_frog_day/images/turn_signal_4.png b/sfa/holiday_themes/world_frog_day/images/turn_signal_4.png new file mode 100644 index 0000000..c3c1d20 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/images/turn_signal_4.png differ diff --git a/sfa/holiday_themes/world_frog_day/sounds/disengage.wav b/sfa/holiday_themes/world_frog_day/sounds/disengage.wav new file mode 100644 index 0000000..d46dd9e Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/sounds/disengage.wav differ diff --git a/sfa/holiday_themes/world_frog_day/sounds/engage.wav b/sfa/holiday_themes/world_frog_day/sounds/engage.wav new file mode 100644 index 0000000..795aa53 Binary files /dev/null and b/sfa/holiday_themes/world_frog_day/sounds/engage.wav differ diff --git a/sfa/other_images/aggressive.png b/sfa/other_images/aggressive.png new file mode 100644 index 0000000..42d0055 Binary files /dev/null and b/sfa/other_images/aggressive.png differ diff --git a/sfa/other_images/aggressive_kaofui.png b/sfa/other_images/aggressive_kaofui.png new file mode 100644 index 0000000..52da0a4 Binary files /dev/null and b/sfa/other_images/aggressive_kaofui.png differ diff --git a/sfa/other_images/bg.jpg b/sfa/other_images/bg.jpg new file mode 100644 index 0000000..13c36a7 Binary files /dev/null and b/sfa/other_images/bg.jpg differ diff --git a/sfa/other_images/brake_pedal.png b/sfa/other_images/brake_pedal.png new file mode 100644 index 0000000..f4fe0ef Binary files /dev/null and b/sfa/other_images/brake_pedal.png differ diff --git a/sfa/other_images/compass_inner.png b/sfa/other_images/compass_inner.png new file mode 100644 index 0000000..aa2d451 Binary files /dev/null and b/sfa/other_images/compass_inner.png differ diff --git a/sfa/other_images/frogpilot_boot_logo.png b/sfa/other_images/frogpilot_boot_logo.png new file mode 100644 index 0000000..2505b8c Binary files /dev/null and b/sfa/other_images/frogpilot_boot_logo.png differ diff --git a/sfa/other_images/gas_pedal.png b/sfa/other_images/gas_pedal.png new file mode 100644 index 0000000..ef1a87e Binary files /dev/null and b/sfa/other_images/gas_pedal.png differ diff --git a/selfdrive/assets/offroad/icon_wifi_uploading_disabled.svg b/sfa/other_images/icon_wifi_uploading_disabled.svg similarity index 100% rename from selfdrive/assets/offroad/icon_wifi_uploading_disabled.svg rename to sfa/other_images/icon_wifi_uploading_disabled.svg diff --git a/sfa/other_images/relaxed.png b/sfa/other_images/relaxed.png new file mode 100644 index 0000000..fb77ec3 Binary files /dev/null and b/sfa/other_images/relaxed.png differ diff --git a/sfa/other_images/relaxed_kaofui.png b/sfa/other_images/relaxed_kaofui.png new file mode 100644 index 0000000..897ad3d Binary files /dev/null and b/sfa/other_images/relaxed_kaofui.png differ diff --git a/sfa/other_images/standard.png b/sfa/other_images/standard.png new file mode 100644 index 0000000..57c5a67 Binary files /dev/null and b/sfa/other_images/standard.png differ diff --git a/sfa/other_images/standard_kaofui.png b/sfa/other_images/standard_kaofui.png new file mode 100644 index 0000000..481e257 Binary files /dev/null and b/sfa/other_images/standard_kaofui.png differ diff --git a/sfa/other_images/traffic.png b/sfa/other_images/traffic.png new file mode 100644 index 0000000..b59f7bc Binary files /dev/null and b/sfa/other_images/traffic.png differ diff --git a/sfa/other_images/traffic_kaofui.png b/sfa/other_images/traffic_kaofui.png new file mode 100644 index 0000000..9930ea8 Binary files /dev/null and b/sfa/other_images/traffic_kaofui.png differ diff --git a/sfa/random_events/images/firefox.png b/sfa/random_events/images/firefox.png new file mode 100644 index 0000000..6c3a418 Binary files /dev/null and b/sfa/random_events/images/firefox.png differ diff --git a/sfa/random_events/images/great_scott.gif b/sfa/random_events/images/great_scott.gif new file mode 100644 index 0000000..a29ca8e Binary files /dev/null and b/sfa/random_events/images/great_scott.gif differ diff --git a/sfa/random_events/images/tree_fiddy.gif b/sfa/random_events/images/tree_fiddy.gif new file mode 100644 index 0000000..3207179 Binary files /dev/null and b/sfa/random_events/images/tree_fiddy.gif differ diff --git a/sfa/random_events/images/weeb_wheel.gif b/sfa/random_events/images/weeb_wheel.gif new file mode 100644 index 0000000..0c8374e Binary files /dev/null and b/sfa/random_events/images/weeb_wheel.gif differ diff --git a/sfa/random_events/sounds/angry.wav b/sfa/random_events/sounds/angry.wav new file mode 100644 index 0000000..6bc6991 Binary files /dev/null and b/sfa/random_events/sounds/angry.wav differ diff --git a/sfa/random_events/sounds/doc.wav b/sfa/random_events/sounds/doc.wav new file mode 100644 index 0000000..9fd679e Binary files /dev/null and b/sfa/random_events/sounds/doc.wav differ diff --git a/sfa/random_events/sounds/fart.wav b/sfa/random_events/sounds/fart.wav new file mode 100644 index 0000000..d825cba Binary files /dev/null and b/sfa/random_events/sounds/fart.wav differ diff --git a/sfa/random_events/sounds/firefox.wav b/sfa/random_events/sounds/firefox.wav new file mode 100644 index 0000000..9b0668f Binary files /dev/null and b/sfa/random_events/sounds/firefox.wav differ diff --git a/sfa/random_events/sounds/nessie.wav b/sfa/random_events/sounds/nessie.wav new file mode 100644 index 0000000..899dc72 Binary files /dev/null and b/sfa/random_events/sounds/nessie.wav differ diff --git a/sfa/random_events/sounds/noice.wav b/sfa/random_events/sounds/noice.wav new file mode 100644 index 0000000..95c8d01 Binary files /dev/null and b/sfa/random_events/sounds/noice.wav differ diff --git a/sfa/random_events/sounds/uwu.wav b/sfa/random_events/sounds/uwu.wav new file mode 100644 index 0000000..830360c Binary files /dev/null and b/sfa/random_events/sounds/uwu.wav differ diff --git a/sfa/toggle_icons/icon_always_on_lateral.png b/sfa/toggle_icons/icon_always_on_lateral.png new file mode 100644 index 0000000..1e55e3f Binary files /dev/null and b/sfa/toggle_icons/icon_always_on_lateral.png differ diff --git a/sfa/toggle_icons/icon_conditional.png b/sfa/toggle_icons/icon_conditional.png new file mode 100644 index 0000000..9834f86 Binary files /dev/null and b/sfa/toggle_icons/icon_conditional.png differ diff --git a/sfa/toggle_icons/icon_custom.png b/sfa/toggle_icons/icon_custom.png new file mode 100644 index 0000000..f1c7f4d Binary files /dev/null and b/sfa/toggle_icons/icon_custom.png differ diff --git a/sfa/toggle_icons/icon_device.png b/sfa/toggle_icons/icon_device.png new file mode 100644 index 0000000..e4f4407 Binary files /dev/null and b/sfa/toggle_icons/icon_device.png differ diff --git a/sfa/toggle_icons/icon_green_light.png b/sfa/toggle_icons/icon_green_light.png new file mode 100644 index 0000000..f43b2ed Binary files /dev/null and b/sfa/toggle_icons/icon_green_light.png differ diff --git a/sfa/toggle_icons/icon_lane.png b/sfa/toggle_icons/icon_lane.png new file mode 100644 index 0000000..cd8e40a Binary files /dev/null and b/sfa/toggle_icons/icon_lane.png differ diff --git a/sfa/toggle_icons/icon_lateral_tune.png b/sfa/toggle_icons/icon_lateral_tune.png new file mode 100644 index 0000000..ba83e3a Binary files /dev/null and b/sfa/toggle_icons/icon_lateral_tune.png differ diff --git a/sfa/toggle_icons/icon_light.png b/sfa/toggle_icons/icon_light.png new file mode 100644 index 0000000..2a3369c Binary files /dev/null and b/sfa/toggle_icons/icon_light.png differ diff --git a/sfa/toggle_icons/icon_longitudinal_tune.png b/sfa/toggle_icons/icon_longitudinal_tune.png new file mode 100644 index 0000000..4af03cd Binary files /dev/null and b/sfa/toggle_icons/icon_longitudinal_tune.png differ diff --git a/sfa/toggle_icons/icon_mute.png b/sfa/toggle_icons/icon_mute.png new file mode 100644 index 0000000..3e31a13 Binary files /dev/null and b/sfa/toggle_icons/icon_mute.png differ diff --git a/sfa/toggle_icons/icon_rotate.png b/sfa/toggle_icons/icon_rotate.png new file mode 100644 index 0000000..1503308 Binary files /dev/null and b/sfa/toggle_icons/icon_rotate.png differ diff --git a/sfa/toggle_icons/icon_speed_map.png b/sfa/toggle_icons/icon_speed_map.png new file mode 100644 index 0000000..60b87eb Binary files /dev/null and b/sfa/toggle_icons/icon_speed_map.png differ diff --git a/sfa/toggle_icons/icon_vtc.png b/sfa/toggle_icons/icon_vtc.png new file mode 100644 index 0000000..8218b45 Binary files /dev/null and b/sfa/toggle_icons/icon_vtc.png differ diff --git a/sfa/toggle_icons/quality_of_life.png b/sfa/toggle_icons/quality_of_life.png new file mode 100644 index 0000000..1719a60 Binary files /dev/null and b/sfa/toggle_icons/quality_of_life.png differ diff --git a/sfa/wheel_images/frog.png b/sfa/wheel_images/frog.png new file mode 100644 index 0000000..f0aca65 Binary files /dev/null and b/sfa/wheel_images/frog.png differ diff --git a/sfa/wheel_images/hyundai.png b/sfa/wheel_images/hyundai.png new file mode 100644 index 0000000..60ca0c8 Binary files /dev/null and b/sfa/wheel_images/hyundai.png differ diff --git a/sfa/wheel_images/lexus.png b/sfa/wheel_images/lexus.png new file mode 100644 index 0000000..1be4431 Binary files /dev/null and b/sfa/wheel_images/lexus.png differ diff --git a/sfa/wheel_images/rocket.png b/sfa/wheel_images/rocket.png new file mode 100644 index 0000000..abe753a Binary files /dev/null and b/sfa/wheel_images/rocket.png differ diff --git a/sfa/wheel_images/stalin.png b/sfa/wheel_images/stalin.png new file mode 100644 index 0000000..f1feebb Binary files /dev/null and b/sfa/wheel_images/stalin.png differ diff --git a/sfa/wheel_images/toyota.png b/sfa/wheel_images/toyota.png new file mode 100644 index 0000000..0e8cf4c Binary files /dev/null and b/sfa/wheel_images/toyota.png differ