diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6758432b68..302b0f9d7d 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -217,7 +217,6 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP']) self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 9973862b85..c82287d2b1 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -53,7 +53,6 @@ class VCruiseHelper(VCruiseHelperSP): if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, _enabled, is_metric) - self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -105,12 +104,6 @@ class VCruiseHelper(VCruiseHelperSP): if not self.button_change_states[button_type]["enabled"]: return - # Speed Limit Assist for Non PCM long cars. - # True: Disallow set speed changes when user confirmed the target set speed during preActive state - # False: Allow set speed changes as SLA is not requesting user confirmation - if self.update_speed_limit_assist_pre_active_confirmed(button_type): - return - long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta) if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index fe7124e5cc..98579704af 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/selfdrive/ui/sunnypilot/ui_state.py @@ -17,7 +17,7 @@ class UIStateSP: self.params = Params() self.sm_services_ext = [ "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", - "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay" + "gpsLocation", "liveTorqueParameters", "carStateSP", "carParamsSP", "liveDelay" ] self.sunnylink_state = SunnylinkState() diff --git a/sunnypilot/mapd/__init__.py b/sunnypilot/mapd/__init__.py deleted file mode 100644 index 7ad6f74149..0000000000 --- a/sunnypilot/mapd/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -import os -from openpilot.common.basedir import BASEDIR - -MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj') -MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd') diff --git a/sunnypilot/mapd/live_map_data/__init__.py b/sunnypilot/mapd/live_map_data/__init__.py deleted file mode 100644 index 557ad06515..0000000000 --- a/sunnypilot/mapd/live_map_data/__init__.py +++ /dev/null @@ -1,22 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from openpilot.common.swaglog import cloudlog - -LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg. -_DEBUG = False -_CLOUDLOG_DEBUG = False -ROAD_NAME_TIMEOUT = 30 # secs -R = 6373000.0 # approximate radius of earth in mts -QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. -QUERY_RADIUS_OFFLINE = 2250 # mts. Radius to use on offline OSM data queries. - - -def get_debug(msg, log_to_cloud=True): - if _CLOUDLOG_DEBUG and log_to_cloud: - cloudlog.debug(msg) - if _DEBUG: - print(msg) diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py deleted file mode 100644 index 723864dd2a..0000000000 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ /dev/null @@ -1,65 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from abc import abstractmethod, ABC - -import cereal.messaging as messaging -from openpilot.common.params import Params -from openpilot.common.constants import CV -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot.navd.helpers import coordinate_from_param - -MAX_SPEED_LIMIT = V_CRUISE_UNSET * CV.KPH_TO_MS - - -class BaseMapData(ABC): - def __init__(self): - self.params = Params() - - self.sm = messaging.SubMaster(['liveLocationKalman']) - self.pm = messaging.PubMaster(['liveMapDataSP']) - - self.localizer_valid = False - self.last_bearing = None - self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params) - - @abstractmethod - def update_location(self) -> None: - pass - - @abstractmethod - def get_current_speed_limit(self) -> float: - pass - - @abstractmethod - def get_next_speed_limit_and_distance(self) -> tuple[float, float]: - pass - - @abstractmethod - def get_current_road_name(self) -> str: - pass - - def publish(self) -> None: - speed_limit = self.get_current_speed_limit() - next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() - - mapd_sp_send = messaging.new_message('liveMapDataSP') - mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK - live_map_data = mapd_sp_send.liveMapDataSP - - live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0) - live_map_data.speedLimit = speed_limit - live_map_data.speedLimitAheadValid = bool(MAX_SPEED_LIMIT > next_speed_limit > 0) - live_map_data.speedLimitAhead = next_speed_limit - live_map_data.speedLimitAheadDistance = next_speed_limit_distance - live_map_data.roadName = self.get_current_road_name() - - self.pm.send('liveMapDataSP', mapd_sp_send) - - def tick(self) -> None: - self.sm.update(0) - self.update_location() - self.publish() diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py deleted file mode 100644 index 794f2ef8ff..0000000000 --- a/sunnypilot/mapd/live_map_data/debug.py +++ /dev/null @@ -1,56 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -# DISCLAIMER: This code is intended principally for development and debugging purposes. -# Although it provides a standalone entry point to the program, users should refer -# to the actual implementations for consumption. Usage outside of development scenarios -# is not advised and could lead to unpredictable results. - -import threading -import traceback - -from cereal import messaging -from openpilot.common.gps import get_gps_location_service -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver -from openpilot.sunnypilot.mapd.live_map_data import get_debug - - -def excepthook(args): - get_debug(f'MapD: Threading exception:\n{args}') - traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback) - - -def live_map_data_sp_thread(): - config_realtime_process([0, 1, 2, 3], 5) - - params = Params() - gps_location_service = get_gps_location_service(params) - - while True: - live_map_data_sp_thread_debug(gps_location_service) - - -def live_map_data_sp_thread_debug(gps_location_service): - _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service]) - _sub_master.update() - - v_ego = _sub_master['carState'].vEgo - _resolver = SpeedLimitResolver() - _resolver.policy = Policy.car_state_priority - _resolver.update(v_ego, _sub_master) - print(_resolver.speed_limit, _resolver.distance, _resolver.source) - - -def main(): - threading.excepthook = excepthook - live_map_data_sp_thread() - - -if __name__ == "__main__": - main() diff --git a/sunnypilot/mapd/live_map_data/osm_map_data.py b/sunnypilot/mapd/live_map_data/osm_map_data.py deleted file mode 100644 index 0662e2d589..0000000000 --- a/sunnypilot/mapd/live_map_data/osm_map_data.py +++ /dev/null @@ -1,61 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import json -import math -import platform - -from cereal import log -from openpilot.common.params import Params -from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData -from openpilot.sunnypilot.navd.helpers import Coordinate - - -class OsmMapData(BaseMapData): - def __init__(self): - super().__init__() - self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params - - def update_location(self) -> None: - location = self.sm['liveLocationKalman'] - self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid - - if self.localizer_valid: - self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) - self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) - - if self.last_position is None: - return - - params = { - "latitude": self.last_position.latitude, - "longitude": self.last_position.longitude, - } - - if self.last_bearing is not None: - params['bearing'] = self.last_bearing - - self.mem_params.put("LastGPSPosition", json.dumps(params)) - - def get_current_speed_limit(self) -> float: - return float(self.mem_params.get("MapSpeedLimit") or 0.0) - - def get_current_road_name(self) -> str: - return str(self.mem_params.get("RoadName") or "") - - def get_next_speed_limit_and_distance(self) -> tuple[float, float]: - next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit") - next_speed_limit_section = next_speed_limit_section_str if next_speed_limit_section_str else {} - next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0) - next_speed_limit_latitude = next_speed_limit_section.get('latitude') - next_speed_limit_longitude = next_speed_limit_section.get('longitude') - next_speed_limit_distance = 0.0 - - if next_speed_limit_latitude and next_speed_limit_longitude: - next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude) - next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates) - - return next_speed_limit, next_speed_limit_distance diff --git a/sunnypilot/mapd/live_map_data/standalone.py b/sunnypilot/mapd/live_map_data/standalone.py deleted file mode 100644 index f73ecc7724..0000000000 --- a/sunnypilot/mapd/live_map_data/standalone.py +++ /dev/null @@ -1,42 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -# DISCLAIMER: This code is intended principally for development and debugging purposes. -# Although it provides a standalone entry point to the program, users should refer -# to the actual implementations for consumption. Usage outside of development scenarios -# is not advised and could lead to unpredictable results. - -import threading -import traceback - -from openpilot.common.realtime import Ratekeeper, config_realtime_process -from openpilot.sunnypilot.mapd.live_map_data import get_debug -from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData - - -def excepthook(args): - get_debug(f'MapD: Threading exception:\n{args}') - traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback) - - -def live_map_data_sp_thread(): - config_realtime_process([0, 1, 2, 3], 5) - - live_map_sp = OsmMapData() - rk = Ratekeeper(1, print_delay_threshold=None) - - while True: - live_map_sp.tick() - rk.keep_time() - - -def main(): - threading.excepthook = excepthook - live_map_data_sp_thread() - - -if __name__ == "__main__": - main() diff --git a/sunnypilot/mapd/mapd_installer.py b/sunnypilot/mapd/mapd_installer.py deleted file mode 100755 index 08d17376d6..0000000000 --- a/sunnypilot/mapd/mapd_installer.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/env python3 -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import logging -import os -import stat -import time -import traceback -import requests -from pathlib import Path -from urllib.request import urlopen - -from cereal import messaging -from openpilot.common.params import Params -from openpilot.system.hardware.hw import Paths -from openpilot.common.spinner import Spinner -from openpilot.system.version import is_prebuilt -from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR -import openpilot.system.sentry as sentry - -VERSION = "v1.12.0" -URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd" - - -def update_installed_version(version: str, params: Params = None) -> None: - if params is None: - params = Params() - - params.put("MapdVersion", version) - - -class MapdInstallManager: - def __init__(self, spinner_ref: Spinner): - self._spinner = spinner_ref - self._params = Params() - - def download(self) -> None: - self.ensure_directories_exist() - self._download_file() - update_installed_version(VERSION, self._params) - - def check_and_download(self) -> None: - if self.download_needed(): - self.download() - - def download_needed(self) -> bool: - return not os.path.exists(MAPD_PATH) or self.get_installed_version() != VERSION - - @staticmethod - def ensure_directories_exist() -> None: - if not os.path.exists(Paths.mapd_root()): - os.makedirs(Paths.mapd_root()) - if not os.path.exists(MAPD_BIN_DIR): - os.makedirs(MAPD_BIN_DIR) - - @staticmethod - def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None: - with open(file_path, 'wb') as output: - output.write(content) - output.flush() - os.fsync(output.fileno()) - current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode) - os.chmod(file_path, current_permissions | stat.S_IEXEC) - - def _download_file(self, num_retries=5) -> None: - temp_file = Path(MAPD_PATH + ".tmp") - download_timeout = 60 - for cnt in range(num_retries): - try: - response = requests.get(URL, stream=True, timeout=download_timeout) - response.raise_for_status() - self._safe_write_and_set_executable(temp_file, response.content) - # No exceptions encountered. Safe to replace original file. - temp_file.replace(MAPD_PATH) - return - except requests.exceptions.ReadTimeout: - self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]") - time.sleep(0.5) - except requests.exceptions.RequestException as e: - self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]") - time.sleep(0.5) - - # Delete temp file if the process was not successful. - if temp_file.exists(): - temp_file.unlink() - logging.error("Failed to download file after all retries") - - def get_installed_version(self) -> str: - return str(self._params.get("MapdVersion") or "") - - def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool: - max_retries = 10 - for retries in range(max_retries + 1): - self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]") - time.sleep(2) - try: - _ = urlopen('https://sentry.io', timeout=10) - return True - except Exception as e: - print(f'Wait for internet failed: {e}') - if return_on_failure and retries == max_retries: - return False - - return False - - def non_prebuilt_install(self) -> None: - sm = messaging.SubMaster(['deviceState']) - metered = sm['deviceState'].networkMetered - - if metered: - self._spinner.update("Can't proceed with mapd install since network is metered!") - time.sleep(5) - return - - try: - self.ensure_directories_exist() - if not self.download_needed(): - self._spinner.update("Mapd is good!") - time.sleep(0.1) - return - - if self.wait_for_internet_connection(return_on_failure=True): - self._spinner.update(f"Downloading pfeiferj's mapd [{self.get_installed_version()}] => [{VERSION}].") - time.sleep(0.1) - self.check_and_download() - self._spinner.close() - - except Exception: - for i in range(6): - self._spinner.update("Failed to download OSM maps won't work until properly downloaded!" + - "Try again manually rebooting. " + - f"Boot will continue in {5 - i}s...") - time.sleep(1) - - sentry.init(sentry.SentryProject.SELFDRIVE) - traceback.print_exc() - sentry.capture_exception() - - -if __name__ == "__main__": - spinner = Spinner() - install_manager = MapdInstallManager(spinner) - install_manager.ensure_directories_exist() - if is_prebuilt(): - debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]" - spinner.update(debug_msg) - update_installed_version(VERSION) - else: - spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]") - install_manager.non_prebuilt_install() diff --git a/sunnypilot/mapd/mapd_manager.py b/sunnypilot/mapd/mapd_manager.py deleted file mode 100755 index 9304f8f0b7..0000000000 --- a/sunnypilot/mapd/mapd_manager.py +++ /dev/null @@ -1,144 +0,0 @@ -#!/usr/bin/env python3 -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import json -import platform -import os -import glob -import shutil -from datetime import datetime - -from openpilot.common.params import Params -from openpilot.common.realtime import Ratekeeper, config_realtime_process -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData -from openpilot.system.hardware.hw import Paths -from openpilot.sunnypilot.mapd import MAPD_PATH -from openpilot.sunnypilot.mapd.mapd_installer import VERSION, update_installed_version - -# PFEIFER - MAPD {{ -params = Params() -mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params -# }} PFEIFER - MAPD - - -def get_files_for_cleanup() -> list[str]: - paths = [ - f"{Paths.mapd_root()}/db", - f"{Paths.mapd_root()}/v*" - ] - files_to_remove = [] - for path in paths: - if os.path.exists(path): - files = glob.glob(path + '/**', recursive=True) - files_to_remove.extend(files) - # check for version and mapd files - if not os.path.isfile(MAPD_PATH): - files_to_remove.append(MAPD_PATH) - return files_to_remove - - -def cleanup_old_osm_data(files_to_remove: list[str]) -> None: - for file in files_to_remove: - # Remove trailing slash if path is file - if file.endswith('/') and os.path.isfile(file[:-1]): - file = file[:-1] - # Try to remove as file or symbolic link first - if os.path.islink(file) or os.path.isfile(file): - os.remove(file) - elif os.path.isdir(file): # If it's a directory - shutil.rmtree(file, ignore_errors=False) - - -def request_refresh_osm_location_data(nations: list[str], states: list[str] = None) -> None: - params.put("OsmDownloadedDate", str(datetime.now().timestamp())) - params.put_bool("OsmDbUpdatesCheck", False) - - osm_download_locations = { - "nations": nations, - "states": states or [] - } - - print(f"Downloading maps for {json.dumps(osm_download_locations)}") - mem_params.put("OSMDownloadLocations", osm_download_locations) - - -def filter_nations_and_states(nations: list[str], states: list[str] = None) -> tuple[list[str], list[str]]: - """Filters and prepares nation and state data for OSM map download. - - If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list. - If the nation is 'US' and the state is 'All', the 'All' is removed from the list. - The idea behind these filters is that if a specific state in the US is provided, - there's no need to download map data for the entire US. Conversely, - if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US, - and 'All' isn't a valid state name, so it's removed. - - Parameters: - nations (list): A list of nations for which the map data is to be downloaded. - states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None. - - Returns: - tuple: Two lists. The first list is filtered nations and the second list is filtered states. - """ - - if "US" in nations and states and not any(x.lower() == "all" for x in states): - # If a specific state in the US is provided, remove 'US' from nations - nations.remove("US") - elif "US" in nations and states and any(x.lower() == "all" for x in states): - # If 'All' is provided as a state (case invariant), remove those instances from states - states = [x for x in states if x.lower() != "all"] - elif "US" not in nations and states and any(x.lower() == "all" for x in states): - states.remove("All") - return nations, states or [] - - -def update_osm_db() -> None: - if params.get_bool("OsmDbUpdatesCheck"): - cleanup_old_osm_data(get_files_for_cleanup()) - country = params.get("OsmLocationName", return_default=True) - state = params.get("OsmStateName", return_default=True) - filtered_nations, filtered_states = filter_nations_and_states([country], [state]) - request_refresh_osm_location_data(filtered_nations, filtered_states) - - if not mem_params.get("OSMDownloadBounds"): - mem_params.put("OSMDownloadBounds", "") - - if not mem_params.get("LastGPSPosition"): - mem_params.put("LastGPSPosition", "{}") - - -def main_thread(): - update_installed_version(VERSION, params) - config_realtime_process([0, 1, 2, 3], 5) - - rk = Ratekeeper(1, print_delay_threshold=None) - live_map_sp = OsmMapData() - - # Create folder needed for OSM - try: - os.mkdir(Paths.mapd_root()) - except FileExistsError: - pass - except PermissionError: - cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}") - - while True: - show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal") - set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.") - - update_osm_db() - live_map_sp.tick() - rk.keep_time() - - -def main(): - main_thread() - - -if __name__ == "__main__": - main() diff --git a/sunnypilot/mapd/tests/__init__.py b/sunnypilot/mapd/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/mapd/tests/mapd_hash b/sunnypilot/mapd/tests/mapd_hash deleted file mode 100644 index 0322ea97ca..0000000000 --- a/sunnypilot/mapd/tests/mapd_hash +++ /dev/null @@ -1 +0,0 @@ -fdb3b49ee19956e6ce09fdc3373cbba557f1263b2180e9f344c1d4053852284b \ No newline at end of file diff --git a/sunnypilot/mapd/tests/test_mapd_version.py b/sunnypilot/mapd/tests/test_mapd_version.py deleted file mode 100644 index 5619d2ec29..0000000000 --- a/sunnypilot/mapd/tests/test_mapd_version.py +++ /dev/null @@ -1,19 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from openpilot.sunnypilot import get_file_hash -from openpilot.sunnypilot.mapd import MAPD_PATH -from openpilot.sunnypilot.mapd.update_version import MAPD_HASH_PATH - - -class TestMapdVersion: - def test_compare_versions(self): - mapd_hash = get_file_hash(MAPD_PATH) - - with open(MAPD_HASH_PATH) as f: - current_hash = f.read().strip() - - assert current_hash == mapd_hash, "Run sunnypilot/mapd/update_version.py to update the current mapd version and hash" diff --git a/sunnypilot/mapd/update_version.py b/sunnypilot/mapd/update_version.py deleted file mode 100755 index c5e08b3f8f..0000000000 --- a/sunnypilot/mapd/update_version.py +++ /dev/null @@ -1,97 +0,0 @@ -#!/usr/bin/env python3 -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import argparse -import os -import re - -from openpilot.sunnypilot import get_file_hash -from openpilot.common.basedir import BASEDIR -from openpilot.sunnypilot.mapd import MAPD_PATH - -MAPD_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "tests", "mapd_hash") -MAPD_VERSION_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "mapd_installer.py") - - -def update_mapd_hash(): - mapd_hash = get_file_hash(MAPD_PATH) - - with open(MAPD_HASH_PATH, "w") as f: - f.write(mapd_hash) - - print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}") - - -def get_current_mapd_version(path: str) -> str: - print("[GET CURRENT MAPD VERSION]") - with open(path) as f: - for line in f: - if line.strip().startswith("VERSION"): - # Match VERSION = 'v1.11.0' or VERSION="v1.11.0" (with optional spaces) - match = re.search(r'VERSION\s*=\s*[\'"]([^\'"]+)[\'"]', line) - if match: - ver = match.group(1) - print(f'Current mapd version: "{ver}"') - return ver - else: - print("[ERROR] VERSION line found but no quoted value detected.") - return "" - print("[ERROR] VERSION not found in file!") - return "" - - -def update_mapd_version(ver: str, path: str): - print("[CHANGE CURRENT MAPD VERSION]") - - with open(path) as f: - lines = f.readlines() - - found = False - new_lines = [] - for line in lines: - if not found and line.startswith("VERSION ="): - new_lines.append(f'VERSION = "{ver}"\n') - found = True - new_lines.extend(lines[lines.index(line) + 1:]) - break - else: - new_lines.append(line) - - if not found: - print("[ERROR] VERSION line not found! Aborting without writing.") - return - - with open(path, "w") as f: - f.writelines(new_lines) - - print(f'New mapd version: "{ver}"') - print("[DONE]") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Update mapd version and hash") - parser.add_argument("--new_ver", type=str, help="New mapd version") - args = parser.parse_args() - - if not args.new_ver: - print("Warning: No new mapd version provided. Use --new_ver to specify") - print("Example:") - print(" python sunnypilot/mapd/update_version.py --new_ver \"v1.12.0\"") - print("Current mapd version and hash will not be updated! (aborted)") - exit(0) - - current_ver = get_current_mapd_version(MAPD_VERSION_PATH) - new_ver = f"{args.new_ver}" - if current_ver == new_ver: - print(f'Proposed mapd version: "{new_ver}"') - confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip() - if confirm != "Y": - print("Current mapd version and hash will not be updated! (aborted)") - exit(0) - - update_mapd_version(new_ver, MAPD_VERSION_PATH) - update_mapd_hash() diff --git a/sunnypilot/mapd/version.py b/sunnypilot/mapd/version.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index 3691c35972..90f39287d2 100644 --- a/sunnypilot/selfdrive/car/cruise_ext.py +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -6,16 +6,12 @@ See the LICENSE.md file in the root directory for more details. """ import numpy as np -from cereal import car, custom +from cereal import car from opendbc.car import structs -from openpilot.common.constants import CV from openpilot.common.params import Params from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target ButtonType = car.CarState.ButtonEvent.Type -SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.setCruise: 0, ButtonType.resumeCruise: 0, @@ -54,16 +50,6 @@ class VCruiseHelperSP: self.enable_button_timers = CRUISE_BUTTON_TIMER - # Speed Limit Assist - self.sla_state = SpeedLimitAssistState.disabled - self.prev_sla_state = SpeedLimitAssistState.disabled - self.has_speed_limit = False - self.speed_limit_final_last = 0. - self.speed_limit_final_last_kph = 0. - self.prev_speed_limit_final_last_kph = 0. - self.req_plus = False - self.req_minus = False - def read_custom_set_speed_params(self) -> None: self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) @@ -106,33 +92,3 @@ class VCruiseHelperSP: return enabled and self.enabled_prev return enabled - - def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None: - resolver = LP_SP.speedLimit.resolver - self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid - self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast - self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH - self.sla_state = LP_SP.speedLimit.assist.state - self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS, - self.speed_limit_final_last, is_metric) - - @property - def update_speed_limit_final_last_changed(self) -> bool: - return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph) - - def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool: - if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive: - if button_type == ButtonType.decelCruise and self.req_minus: - return True - if button_type == ButtonType.accelCruise and self.req_plus: - return True - - return False - - def update_speed_limit_assist_v_cruise_non_pcm(self) -> None: - if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or - self.update_speed_limit_final_last_changed): - self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX) - - self.prev_sla_state = self.sla_state - self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index a93f5724b5..3fe3b3b776 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -11,7 +11,6 @@ from opendbc.car.interfaces import CarInterfaceBase from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability import openpilot.system.sentry as sentry @@ -86,10 +85,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params") params.remove("DynamicExperimentalControl") params.remove("CustomAccIncrementsEnabled") - params.remove("SmartCruiseControlVision") - params.remove("SmartCruiseControlMap") - - set_speed_limit_assist_availability(CP, CP_SP, params) def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None: diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py deleted file mode 100644 index 271f49dcc2..0000000000 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from openpilot.common.constants import CV - -MIN_V = 20 * CV.KPH_TO_MS # Do not operate under 20 km/h diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py deleted file mode 100644 index c7f11a1bb2..0000000000 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py +++ /dev/null @@ -1,261 +0,0 @@ -import json -import math -import platform - -from cereal import custom -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V - -MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState - -ACTIVE_STATES = (MapState.turning, ) -ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES) - -R = 6373000.0 # approximate radius of earth in meters -TO_RADIANS = math.pi / 180 -TO_DEGREES = 180 / math.pi -TARGET_JERK = -0.6 # m/s^3 There's some jounce limits that are not consistent so we're fudging this some -TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit -TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps - # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot. - # The value is multiplied against the target velocity to determine the additional distance. This is - # done to keep the distance calculations consistent but results in the offset actually being less - # time than specified depending on how much of a speed differential there is between v_ego and the - # target velocity. - - -def velocities_from_param(param: str, params: Params): - if params is None: - params = Params() - - json_str = params.get(param) - if json_str is None: - return None - - velocities = json.loads(json_str) - - return velocities - - -def calculate_accel(t, target_jerk, a_ego): - return a_ego + target_jerk * t - - -def calculate_velocity(t, target_jerk, a_ego, v_ego): - return v_ego + a_ego * t + target_jerk/2 * (t ** 2) - - -def calculate_distance(t, target_jerk, a_ego, v_ego): - return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3) - - -# points should be in radians -# output is meters -def distance_to_point(ax, ay, bx, by): - a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2) - c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) - - return R * c # in meters - - -class SmartCruiseControlMap: - v_target: float = 0 - a_target: float = 0. - v_ego: float = 0. - a_ego: float = 0. - output_v_target: float = V_CRUISE_UNSET - output_a_target: float = 0. - - def __init__(self): - self.params = Params() - self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params - self.enabled = self.params.get_bool("SmartCruiseControlMap") - self.long_enabled = False - self.long_override = False - self.is_enabled = False - self.is_active = False - self.state = MapState.disabled - self.v_cruise = 0 - self.target_lat = 0.0 - self.target_lon = 0.0 - self.frame = -1 - - self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0) - self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or [] - - def get_v_target_from_control(self) -> float: - if self.is_active: - return max(self.v_target, MIN_V) - - return V_CRUISE_UNSET - - def get_a_target_from_control(self) -> float: - return self.a_ego - - def update_params(self): - if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.enabled = self.params.get_bool("SmartCruiseControlMap") - - def update_calculations(self) -> None: - self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0) - lat = self.last_position.latitude - lon = self.last_position.longitude - - self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or [] - - if self.last_position is None or self.target_velocities is None: - return - - min_dist = 1000 - min_idx = 0 - distances = [] - - # find our location in the path - for i in range(len(self.target_velocities)): - target_velocity = self.target_velocities[i] - tlat = target_velocity["latitude"] - tlon = target_velocity["longitude"] - d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS) - distances.append(d) - if d < min_dist: - min_dist = d - min_idx = i - - # only look at values from our current position forward - forward_points = self.target_velocities[min_idx:] - forward_distances = distances[min_idx:] - - # find velocities that we are within the distance we need to adjust for - valid_velocities = [] - for i in range(len(forward_points)): - target_velocity = forward_points[i] - tlat = target_velocity["latitude"] - tlon = target_velocity["longitude"] - tv = target_velocity["velocity"] - if tv > self.v_ego: - continue - - d = forward_distances[i] - - a_diff = (self.a_ego - TARGET_ACCEL) - accel_t = abs(a_diff / TARGET_JERK) - min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego) - - max_d = 0 - if tv > min_accel_v: - # calculate time needed based on target jerk - a = 0.5 * TARGET_JERK - b = self.a_ego - c = self.v_ego - tv - t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a - t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a - if not isinstance(t_a, complex) and t_a > 0: - t = t_a - else: - t = t_b - if isinstance(t, complex): - continue - - max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego) - else: - t = accel_t - max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego) - - # calculate additional time needed based on target accel - t = abs((min_accel_v - tv) / TARGET_ACCEL) - max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v) - - if d < max_d + tv * TARGET_OFFSET: - valid_velocities.append((float(tv), tlat, tlon)) - - # Find the smallest velocity we need to adjust for - min_v = 100.0 - target_lat = 0.0 - target_lon = 0.0 - for tv, lat, lon in valid_velocities: - if tv < min_v: - min_v = tv - target_lat = lat - target_lon = lon - - if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0): - for i in range(len(forward_points)): - target_velocity = forward_points[i] - tlat = target_velocity["latitude"] - tlon = target_velocity["longitude"] - tv = target_velocity["velocity"] - if tv > self.v_ego: - continue - - if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target: - return - - # not found so let's reset - self.v_target = 0.0 - self.target_lat = 0.0 - self.target_lon = 0.0 - - self.v_target = min_v - self.target_lat = target_lat - self.target_lon = target_lon - - def _update_state_machine(self) -> tuple[bool, bool]: - # ENABLED, TURNING - if self.state != MapState.disabled: - if not self.long_enabled or not self.enabled: - self.state = MapState.disabled - elif self.long_override: - self.state = MapState.overriding - - else: - # ENABLED - if self.state == MapState.enabled: - if self.v_cruise > self.v_target != 0: - self.state = MapState.turning - - # TURNING - elif self.state == MapState.turning: - if self.v_cruise <= self.v_target or self.v_target == 0: - self.state = MapState.enabled - - # OVERRIDING - elif self.state == MapState.overriding: - if not self.long_override: - if self.v_cruise > self.v_target != 0: - self.state = MapState.turning - else: - self.state = MapState.enabled - - # DISABLED - elif self.state == MapState.disabled: - if self.long_enabled and self.enabled: - if self.long_override: - self.state = MapState.overriding - else: - self.state = MapState.enabled - - enabled = self.state in ENABLED_STATES - active = self.state in ACTIVE_STATES - - return enabled, active - - def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None: - self.long_enabled = long_enabled - self.long_override = long_override - self.v_ego = v_ego - self.a_ego = a_ego - self.v_cruise = v_cruise - - self.update_params() - self.update_calculations() - - self.is_enabled, self.is_active = self._update_state_machine() - - self.output_v_target = self.get_v_target_from_control() - self.output_a_target = self.get_a_target_from_control() - - self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py deleted file mode 100644 index 4ca45202fc..0000000000 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py +++ /dev/null @@ -1,19 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import cereal.messaging as messaging -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap - - -class SmartCruiseControl: - def __init__(self): - self.vision = SmartCruiseControlVision() - self.map = SmartCruiseControlMap() - - def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None: - self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise) - self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py deleted file mode 100644 index 537f0033f0..0000000000 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py +++ /dev/null @@ -1,58 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import platform - -from cereal import custom -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap - -MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState - - -class TestSmartCruiseControlMap: - - def setup_method(self): - self.params = Params() - self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params - self.reset_params() - self.scc_m = SmartCruiseControlMap() - - def reset_params(self): - self.params.put_bool("SmartCruiseControlMap", True) - - # TODO-SP: mock data from gpsLocation - self.params.put("LastGPSPosition", "{}") - self.params.put("MapTargetVelocities", "{}") - - def test_initial_state(self): - assert self.scc_m.state == VisionState.disabled - assert not self.scc_m.is_active - assert self.scc_m.output_v_target == V_CRUISE_UNSET - assert self.scc_m.output_a_target == 0. - - def test_system_disabled(self): - self.params.put_bool("SmartCruiseControlMap", False) - self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap") - - for _ in range(int(10. / DT_MDL)): - self.scc_m.update(True, False, 0., 0., 0.) - assert self.scc_m.state == VisionState.disabled - assert not self.scc_m.is_active - - def test_disabled(self): - for _ in range(int(10. / DT_MDL)): - self.scc_m.update(False, False, 0., 0., 0.) - assert self.scc_m.state == VisionState.disabled - - def test_transition_disabled_to_enabled(self): - for _ in range(int(10. / DT_MDL)): - self.scc_m.update(True, False, 0., 0., 0.) - assert self.scc_m.state == VisionState.enabled - - # TODO-SP: mock data from modelV2 to test other states diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py deleted file mode 100644 index 9f6efffb55..0000000000 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py +++ /dev/null @@ -1,104 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import numpy as np - -import cereal.messaging as messaging -from cereal import custom, log -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision - -VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState - - -def generate_modelV2(): - model = messaging.new_message('modelV2') - position = log.XYZTData.new_message() - speed = 30 - position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)] - model.modelV2.position = position - orientation = log.XYZTData.new_message() - curvature = 0.05 - orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS] - orientation.y = [0.0 for _ in ModelConstants.T_IDXS] - model.modelV2.orientation = orientation - orientationRate = log.XYZTData.new_message() - orientationRate.z = [float(z) for z in ModelConstants.T_IDXS] - model.modelV2.orientationRate = orientationRate - velocity = log.XYZTData.new_message() - velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] - velocity.x[0] = float(speed) # always start at current speed - model.modelV2.velocity = velocity - acceleration = log.XYZTData.new_message() - acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] - acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)] - model.modelV2.acceleration = acceleration - - return model - - -def generate_carState(): - car_state = messaging.new_message('carState') - speed = 30 - v_cruise = 50 - car_state.carState.vEgo = float(speed) - car_state.carState.standstill = False - car_state.carState.vCruise = float(v_cruise * 3.6) - - return car_state - - -def generate_controlsState(): - controls_state = messaging.new_message('controlsState') - controls_state.controlsState.curvature = 0.05 - - return controls_state - - -class TestSmartCruiseControlVision: - - def setup_method(self): - self.params = Params() - self.reset_params() - self.scc_v = SmartCruiseControlVision() - - mdl = generate_modelV2() - cs = generate_carState() - controls_state = generate_controlsState() - self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState} - - def reset_params(self): - self.params.put_bool("SmartCruiseControlVision", True) - - def test_initial_state(self): - assert self.scc_v.state == VisionState.disabled - assert not self.scc_v.is_active - assert self.scc_v.output_v_target == V_CRUISE_UNSET - assert self.scc_v.output_a_target == 0. - - def test_system_disabled(self): - self.params.put_bool("SmartCruiseControlVision", False) - self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision") - - for _ in range(int(10. / DT_MDL)): - self.scc_v.update(self.sm, True, False, 0., 0., 0.) - assert self.scc_v.state == VisionState.disabled - assert not self.scc_v.is_active - - def test_disabled(self): - for _ in range(int(10. / DT_MDL)): - self.scc_v.update(self.sm, False, False, 0., 0., 0.) - assert self.scc_v.state == VisionState.disabled - - def test_transition_disabled_to_enabled(self): - for _ in range(int(10. / DT_MDL)): - self.scc_v.update(self.sm, True, False, 0., 0., 0.) - assert self.scc_v.state == VisionState.enabled - - # TODO-SP: mock modelV2 data to test other states diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py deleted file mode 100644 index d144178e93..0000000000 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ /dev/null @@ -1,203 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import numpy as np - -import cereal.messaging as messaging -from cereal import custom -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V - -VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState - -ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving) -ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES) - -_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state. -_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops. - -_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state. - -_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state. -_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle. - -_A_LAT_REG_MAX = 2. # Maximum lateral acceleration - -_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting. - -# Lookup table for the minimum smooth deceleration during the ENTERING state -# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead. -_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state -_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead - -# Lookup table for the acceleration for the TURNING state -# depending on the current lateral acceleration of the vehicle. -_TURNING_ACC_V = [0.5, 0., -0.4] # acc value -_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc - -_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn. - - -class SmartCruiseControlVision: - v_target: float = 0 - a_target: float = 0. - v_ego: float = 0. - a_ego: float = 0. - output_v_target: float = V_CRUISE_UNSET - output_a_target: float = 0. - - def __init__(self): - self.params = Params() - self.frame = -1 - self.long_enabled = False - self.long_override = False - self.is_enabled = False - self.is_active = False - self.enabled = self.params.get_bool("SmartCruiseControlVision") - self.v_cruise_setpoint = 0. - - self.state = VisionState.disabled - self.current_lat_acc = 0. - self.max_pred_lat_acc = 0. - - def get_a_target_from_control(self) -> float: - return self.a_target - - def get_v_target_from_control(self) -> float: - if self.is_active: - return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON - - return V_CRUISE_UNSET - - def _update_params(self) -> None: - if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.enabled = self.params.get_bool("SmartCruiseControlVision") - - def _update_calculations(self, sm: messaging.SubMaster) -> None: - if not self.long_enabled: - return - else: - rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z)) - vel_plan = np.array(sm['modelV2'].velocity.x) - - self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature) - - # get the maximum lat accel from the model - predicted_lat_accels = rate_plan * vel_plan - self.max_pred_lat_acc = np.amax(predicted_lat_accels) - - # get the maximum curve based on the current velocity - v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations - max_curve = self.max_pred_lat_acc / (v_ego**2) - - # Get the target velocity for the maximum curve - self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5 - - def _update_state_machine(self) -> tuple[bool, bool]: - # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING - if self.state != VisionState.disabled: - # longitudinal and feature disable always have priority in a non-disabled state - if not self.long_enabled or not self.enabled: - self.state = VisionState.disabled - elif self.long_override: - self.state = VisionState.overriding - - else: - # ENABLED - if self.state == VisionState.enabled: - # Do not enter a turn control cycle if the speed is low. - if self.v_ego <= MIN_V: - pass - # If significant lateral acceleration is predicted ahead, then move to Entering turn state. - elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: - self.state = VisionState.entering - - # OVERRIDING - elif self.state == VisionState.overriding: - if not self.long_override: - self.state = VisionState.enabled - - # ENTERING - elif self.state == VisionState.entering: - # Transition to Turning if current lateral acceleration is over the threshold. - if self.current_lat_acc >= _TURNING_LAT_ACC_TH: - self.state = VisionState.turning - # Abort if the predicted lateral acceleration drops - elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH: - self.state = VisionState.enabled - - # TURNING - elif self.state == VisionState.turning: - # Transition to Leaving if current lateral acceleration drops below a threshold. - if self.current_lat_acc <= _LEAVING_LAT_ACC_TH: - self.state = VisionState.leaving - - # LEAVING - elif self.state == VisionState.leaving: - # Transition back to Turning if current lateral acceleration goes back over the threshold. - if self.current_lat_acc >= _TURNING_LAT_ACC_TH: - self.state = VisionState.turning - # Finish if current lateral acceleration goes below a threshold. - elif self.current_lat_acc < _FINISH_LAT_ACC_TH: - self.state = VisionState.enabled - - # DISABLED - elif self.state == VisionState.disabled: - if self.long_enabled and self.enabled: - if self.long_override: - self.state = VisionState.overriding - else: - self.state = VisionState.enabled - - enabled = self.state in ENABLED_STATES - active = self.state in ACTIVE_STATES - - return enabled, active - - def _update_solution(self) -> float: - # DISABLED, ENABLED, OVERRIDING - if self.state not in ACTIVE_STATES: - # when not overshooting, calculate v_turn as the speed at the prediction horizon when following - # the smooth deceleration. - a_target = self.a_ego - # ENTERING - elif self.state == VisionState.entering: - # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come. - a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V) - # TURNING - elif self.state == VisionState.turning: - # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt. - a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V) - # LEAVING - elif self.state == VisionState.leaving: - # When leaving, we provide a comfortable acceleration to regain speed. - a_target = _LEAVING_ACC - else: - raise NotImplementedError(f"SCC-V state not supported: {self.state}") - - return a_target - - def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, - v_cruise_setpoint: float) -> None: - self.long_enabled = long_enabled - self.long_override = long_override - self.v_ego = v_ego - self.a_ego = a_ego - self.v_cruise_setpoint = v_cruise_setpoint - - self._update_params() - self._update_calculations(sm) - - self.is_enabled, self.is_active = self._update_state_machine() - self.a_target = self._update_solution() - - self.output_v_target = self.get_v_target_from_control() - self.output_a_target = self.get_a_target_from_control() - - self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py deleted file mode 100644 index 22ea75fe19..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. -LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. - -# Speed Limit Assist constants -PCM_LONG_REQUIRED_MAX_SET_SPEED = { - True: (33.3333, 36.1111), # km/h, (120, 130) - False: (31.2928, 35.7632), # mph, (70, 80) -} - -CONFIRM_SPEED_THRESHOLD = { - True: 80, # km/h - False: 50, # mph -} diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py deleted file mode 100644 index c46768464e..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py +++ /dev/null @@ -1,29 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" - -from openpilot.sunnypilot import IntEnumBase - - -class Policy(IntEnumBase): - car_state_only = 0 - map_data_only = 1 - car_state_priority = 2 - map_data_priority = 3 - combined = 4 - - -class OffsetType(IntEnumBase): - off = 0 - fixed = 1 - percentage = 2 - - -class Mode(IntEnumBase): - off = 0 - information = 1 - warning = 2 - assist = 3 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py deleted file mode 100644 index 4f388befc7..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py +++ /dev/null @@ -1,44 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" - -from cereal import custom, car -from openpilot.common.constants import CV -from openpilot.common.params import Params -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode - - -def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]: - speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH - v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv) - target_set_speed_conv = round(target_set_speed * speed_conv) - - req_plus = v_cruise_cluster_conv < target_set_speed_conv - req_minus = v_cruise_cluster_conv > target_set_speed_conv - - return req_plus, req_minus - - -def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool: - if params is None: - params = Params() - - is_release = params.get_bool("IsReleaseSpBranch") - disallow_in_release = CP.brand == "tesla" and is_release - always_disallow = CP.brand == "rivian" - allowed = True - - if disallow_in_release or always_disallow: - allowed = False - - if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed: - allowed = False - - if not allowed: - if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist: - params.put("SpeedLimitMode", int(SpeedLimitMode.warning)) - - return allowed diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py deleted file mode 100644 index ff7be8a8be..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ /dev/null @@ -1,414 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import time - -from cereal import custom, car -from openpilot.common.params import Params -from openpilot.common.constants import CV -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability - -ButtonType = car.CarState.ButtonEvent.Type -EventNameSP = custom.OnroadEventSP.EventName -SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source - -ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting) -ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES) - -DISABLED_GUARD_PERIOD = 0.5 # secs. -# secs. Time to wait after activation before considering temp deactivation signal. -PRE_ACTIVE_GUARD_PERIOD = { - True: 15, - False: 5, -} -SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive. - -LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. -LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. -LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. -LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. -V_CRUISE_UNSET = 255. - -CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise) -CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise) -CRUISE_BUTTON_CONFIRM_HOLD = 0.5 # secs. - - -class SpeedLimitAssist: - _speed_limit_final_last: float - _distance: float - v_ego: float - a_ego: float - v_offset: float - - def __init__(self, CP: car.CarParams, CP_SP: custom.CarParamsSP): - self.params = Params() - self.CP = CP - self.CP_SP = CP_SP - self.frame = -1 - self.long_engaged_timer = 0 - self.pre_active_timer = 0 - self.is_metric = self.params.get_bool("IsMetric") - set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params) - self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist - self.long_enabled = False - self.long_enabled_prev = False - self.is_enabled = False - self.is_active = False - self.output_v_target = V_CRUISE_UNSET - self.output_a_target = 0. - self.v_ego = 0. - self.a_ego = 0. - self.v_offset = 0. - self.target_set_speed_conv = 0 - self.prev_target_set_speed_conv = 0 - self.v_cruise_cluster = 0. - self.v_cruise_cluster_prev = 0. - self.v_cruise_cluster_conv = 0 - self.prev_v_cruise_cluster_conv = 0 - self._has_speed_limit = False - self._speed_limit = 0. - self._speed_limit_final_last = 0. - self.speed_limit_prev = 0. - self.speed_limit_final_last_conv = 0 - self.prev_speed_limit_final_last_conv = 0 - self._distance = 0. - self.state = SpeedLimitAssistState.disabled - self._state_prev = SpeedLimitAssistState.disabled - self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise - - self._plus_hold = 0. - self._minus_hold = 0. - self._last_carstate_ts = 0. - - # TODO-SP: SLA's own output_a_target for planner - # Solution functions mapped to respective states - self.acceleration_solutions = { - SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target, - SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target, - SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target, - SpeedLimitAssistState.pending: self.get_current_acceleration_as_target, - SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration, - SpeedLimitAssistState.active: self.get_active_state_target_acceleration, - } - - @property - def speed_limit_changed(self) -> bool: - return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev) - - @property - def v_cruise_cluster_changed(self) -> bool: - return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv) - - @property - def target_set_speed_confirmed(self) -> bool: - return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv) - - @property - def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool: - return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]) - - def update_active_event(self, events_sp: EventsSP) -> None: - if self.v_cruise_cluster_below_confirm_speed_threshold: - events_sp.add(EventNameSP.speedLimitChanged) - else: - events_sp.add(EventNameSP.speedLimitActive) - - def get_v_target_from_control(self) -> float: - if self._has_speed_limit: - if self.pcm_op_long and self.is_enabled: - return self._speed_limit_final_last - if not self.pcm_op_long and self.is_active: - return self._speed_limit_final_last - - # Fallback - return V_CRUISE_UNSET - - # TODO-SP: SLA's own output_a_target for planner - def get_a_target_from_control(self) -> float: - return self.a_ego - - def update_params(self) -> None: - if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.is_metric = self.params.get_bool("IsMetric") - set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params) - self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist - - def update_car_state(self, CS: car.CarState) -> None: - now = time.monotonic() - self._last_carstate_ts = now - - for b in CS.buttonEvents: - if not b.pressed: - if b.type in CRUISE_BUTTONS_PLUS: - self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD) - elif b.type in CRUISE_BUTTONS_MINUS: - self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD) - - def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool: - now = time.monotonic() - if req_plus and now <= self._plus_hold: - self._plus_hold = 0. - return True - elif req_minus and now <= self._minus_hold: - self._minus_hold = 0. - return True - - # expired - if now > self._plus_hold: - self._plus_hold = 0. - if now > self._minus_hold: - self._minus_hold = 0. - return False - - def update_calculations(self, v_cruise_cluster: float) -> None: - speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH - self.v_cruise_cluster = v_cruise_cluster - - # Update current velocity offset (error) - self.v_offset = self._speed_limit_final_last - self.v_ego - - self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv) - self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv) - - cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] - pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \ - cst_high - pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) - - self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv - - @property - def apply_confirm_speed_threshold(self) -> bool: - # below CST: always require user confirmation - if self.v_cruise_cluster_below_confirm_speed_threshold: - return True - - # at/above CST: - # - new speed limit >= CST: auto change - # - new speed limit < CST: user confirmation required - return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]) - - def get_current_acceleration_as_target(self) -> float: - return self.a_ego - - def get_adapting_state_target_acceleration(self) -> float: - if self._distance > 0: - return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance) - - return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) - - def get_active_state_target_acceleration(self) -> float: - return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) - - def _update_confirmed_state(self): - if self._has_speed_limit: - if self.v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitAssistState.adapting - else: - self.state = SpeedLimitAssistState.active - else: - self.state = SpeedLimitAssistState.pending - - def _update_non_pcm_long_confirmed_state(self) -> bool: - if self.target_set_speed_confirmed: - return True - - if self.state != SpeedLimitAssistState.preActive: - return False - - req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric) - - return self._get_button_release(req_plus, req_minus) - - def update_state_machine_pcm_op_long(self): - self.long_engaged_timer = max(0, self.long_engaged_timer - 1) - self.pre_active_timer = max(0, self.pre_active_timer - 1) - - # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE - if self.state != SpeedLimitAssistState.disabled: - if not self.long_enabled or not self.enabled: - self.state = SpeedLimitAssistState.disabled - - else: - # ACTIVE - if self.state == SpeedLimitAssistState.active: - if self.v_cruise_cluster_changed: - self.state = SpeedLimitAssistState.inactive - elif self.speed_limit_changed and self.apply_confirm_speed_threshold: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitAssistState.adapting - - # ADAPTING - elif self.state == SpeedLimitAssistState.adapting: - if self.v_cruise_cluster_changed: - self.state = SpeedLimitAssistState.inactive - elif self.speed_limit_changed and self.apply_confirm_speed_threshold: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - elif self.v_offset >= LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitAssistState.active - - # PENDING - elif self.state == SpeedLimitAssistState.pending: - if self.target_set_speed_confirmed: - self._update_confirmed_state() - elif self.speed_limit_changed: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - - # PRE_ACTIVE - elif self.state == SpeedLimitAssistState.preActive: - if self.target_set_speed_confirmed: - self._update_confirmed_state() - elif self.pre_active_timer <= 0: - # Timeout - session ended - self.state = SpeedLimitAssistState.inactive - - # INACTIVE - elif self.state == SpeedLimitAssistState.inactive: - pass - - # DISABLED - elif self.state == SpeedLimitAssistState.disabled: - if self.long_enabled and self.enabled: - # start or reset preActive timer if initially enabled or manual set speed change detected - if not self.long_enabled_prev or self.v_cruise_cluster_changed: - self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) - - elif self.long_engaged_timer <= 0: - if self.target_set_speed_confirmed: - self._update_confirmed_state() - elif self._has_speed_limit: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - else: - self.state = SpeedLimitAssistState.pending - - enabled = self.state in ENABLED_STATES - active = self.state in ACTIVE_STATES - - return enabled, active - - def update_state_machine_non_pcm_long(self): - self.long_engaged_timer = max(0, self.long_engaged_timer - 1) - self.pre_active_timer = max(0, self.pre_active_timer - 1) - - # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE - if self.state != SpeedLimitAssistState.disabled: - if not self.long_enabled or not self.enabled: - self.state = SpeedLimitAssistState.disabled - - else: - # ACTIVE - if self.state == SpeedLimitAssistState.active: - if self.v_cruise_cluster_changed: - self.state = SpeedLimitAssistState.inactive - - elif self.speed_limit_changed and self.apply_confirm_speed_threshold: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - - # PRE_ACTIVE - elif self.state == SpeedLimitAssistState.preActive: - if self._update_non_pcm_long_confirmed_state(): - self.state = SpeedLimitAssistState.active - elif self.pre_active_timer <= 0: - # Timeout - session ended - self.state = SpeedLimitAssistState.inactive - - # INACTIVE - elif self.state == SpeedLimitAssistState.inactive: - if self.speed_limit_changed: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - elif self._update_non_pcm_long_confirmed_state(): - self.state = SpeedLimitAssistState.active - - # DISABLED - elif self.state == SpeedLimitAssistState.disabled: - if self.long_enabled and self.enabled: - # start or reset preActive timer if initially enabled or manual set speed change detected - if not self.long_enabled_prev or self.v_cruise_cluster_changed: - self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) - - elif self.long_engaged_timer <= 0: - if self._update_non_pcm_long_confirmed_state(): - self.state = SpeedLimitAssistState.active - elif self._has_speed_limit: - self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) - else: - self.state = SpeedLimitAssistState.inactive - - enabled = self.state in ENABLED_STATES - active = self.state in ACTIVE_STATES - - return enabled, active - - def update_events(self, events_sp: EventsSP) -> None: - if self.state == SpeedLimitAssistState.preActive: - events_sp.add(EventNameSP.speedLimitPreActive) - - if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending: - events_sp.add(EventNameSP.speedLimitPending) - - if self.is_active: - if self._state_prev not in ACTIVE_STATES: - self.update_active_event(events_sp) - - # only notify if we acquire a valid speed limit - # do not check has_speed_limit here - elif self._speed_limit != self.speed_limit_prev: - if self.speed_limit_prev <= 0: - self.update_active_event(events_sp) - elif self.speed_limit_prev > 0 and self._speed_limit > 0: - self.update_active_event(events_sp) - - def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float, - speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None: - self.long_enabled = long_enabled - self.v_ego = v_ego - self.a_ego = a_ego - - self._has_speed_limit = has_speed_limit - self._speed_limit = speed_limit - self._speed_limit_final_last = speed_limit_final_last - self._distance = distance - - self.update_params() - self.update_calculations(v_cruise_cluster) - - self._state_prev = self.state - if self.pcm_op_long: - self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long() - else: - self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long() - - self.update_events(events_sp) - - # Update change tracking variables - self.speed_limit_prev = self._speed_limit - self.v_cruise_cluster_prev = self.v_cruise_cluster - self.long_enabled_prev = self.long_enabled - self.prev_target_set_speed_conv = self.target_set_speed_conv - self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv - self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv - - self.output_v_target = self.get_v_target_from_control() - self.output_a_target = self.get_a_target_from_control() - - self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py deleted file mode 100644 index 35965c0e18..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py +++ /dev/null @@ -1,190 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import time - -import cereal.messaging as messaging -from cereal import custom -from openpilot.common.constants import CV -from openpilot.common.gps import get_gps_location_service -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType - -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source - -ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values()) - - -class SpeedLimitResolver: - limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] - distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] - v_ego: float - speed_limit: float - speed_limit_last: float - speed_limit_final: float - speed_limit_final_last: float - distance: float - source: custom.LongitudinalPlanSP.SpeedLimit.Source - speed_limit_offset: float - - def __init__(self): - self.params = Params() - self.frame = -1 - - self._gps_location_service = get_gps_location_service(self.params) - self.limit_solutions = {} # Store for speed limit solutions from different sources - self.distance_solutions = {} # Store for distance to current speed limit start for different sources - - self.policy = self.params.get("SpeedLimitPolicy", return_default=True) - self.policy = get_sanitize_int_param( - "SpeedLimitPolicy", - Policy.min().value, - Policy.max().value, - self.params - ) - self._policy_to_sources_map = { - Policy.car_state_only: [SpeedLimitSource.car], - Policy.map_data_only: [SpeedLimitSource.map], - Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map], - Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car], - Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map], - } - self.source = SpeedLimitSource.none - for source in ALL_SOURCES: - self._reset_limit_sources(source) - - self.is_metric = self.params.get_bool("IsMetric") - self.offset_type = get_sanitize_int_param( - "SpeedLimitOffsetType", - OffsetType.min().value, - OffsetType.max().value, - self.params - ) - self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) - - self.speed_limit = 0. - self.speed_limit_last = 0. - self.speed_limit_final = 0. - self.speed_limit_final_last = 0. - self.speed_limit_offset = 0. - - def update_speed_limit_states(self) -> None: - self.speed_limit_final = self.speed_limit + self.speed_limit_offset - - if self.speed_limit > 0.: - self.speed_limit_last = self.speed_limit - self.speed_limit_final_last = self.speed_limit_final - - @property - def speed_limit_valid(self) -> bool: - return self.speed_limit > 0. - - @property - def speed_limit_last_valid(self) -> bool: - return self.speed_limit_last > 0. - - def update_params(self): - if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.policy = self.params.get("SpeedLimitPolicy", return_default=True) - self.is_metric = self.params.get_bool("IsMetric") - self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) - self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) - - def _get_speed_limit_offset(self) -> float: - if self.offset_type == OffsetType.off: - return 0 - elif self.offset_type == OffsetType.fixed: - return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)) - elif self.offset_type == OffsetType.percentage: - return float(self.offset_value * 0.01 * self.speed_limit) - else: - raise NotImplementedError("Offset not supported") - - def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None: - self.limit_solutions[source] = 0. - self.distance_solutions[source] = 0. - - def _get_from_car_state(self, sm: messaging.SubMaster) -> None: - self._reset_limit_sources(SpeedLimitSource.car) - self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit - self.distance_solutions[SpeedLimitSource.car] = 0. - - def _get_from_map_data(self, sm: messaging.SubMaster) -> None: - self._reset_limit_sources(SpeedLimitSource.map) - self._process_map_data(sm) - - def _process_map_data(self, sm: messaging.SubMaster) -> None: - gps_data = sm[self._gps_location_service] - map_data = sm['liveMapDataSP'] - - gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3 - if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: - return - - speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. - next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. - - self._calculate_map_data_limits(sm, speed_limit, next_speed_limit) - - def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None: - gps_data = sm[self._gps_location_service] - map_data = sm['liveMapDataSP'] - - distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3) - distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) - - self.limit_solutions[SpeedLimitSource.map] = speed_limit - self.distance_solutions[SpeedLimitSource.map] = 0. - - # FIXME-SP: this is not working as expected - if 0. < next_speed_limit < self.v_ego: - adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC - adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2 - - if distance_to_speed_limit_ahead <= adapt_distance: - self.limit_solutions[SpeedLimitSource.map] = next_speed_limit - self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead - - def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source: - sources_for_policy = self._policy_to_sources_map[self.policy] - - if self.policy != Policy.combined: - # They are ordered in the order of preference, so we pick the first that's non-zero - for source in sources_for_policy: - if self.limit_solutions[source] > 0.: - return source - return SpeedLimitSource.none - - sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.] - if sources_with_limits: - return min(sources_with_limits, key=lambda x: x[1])[0] - - return SpeedLimitSource.none - - def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]: - """Get limit solutions from each data source""" - self._get_from_car_state(sm) - self._get_from_map_data(sm) - - source = self._get_source_solution_according_to_policy() - speed_limit = self.limit_solutions[source] if source else 0. - distance = self.distance_solutions[source] if source else 0. - - return speed_limit, distance, source - - def update(self, v_ego: float, sm: messaging.SubMaster) -> None: - self.v_ego = v_ego - self.update_params() - - self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm) - self.speed_limit_offset = self._get_speed_limit_offset() - - self.update_speed_limit_states() - - self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py deleted file mode 100644 index aa6650adda..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ /dev/null @@ -1,278 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" - -import pytest - -from cereal import custom -from opendbc.car.car_helpers import interfaces -from opendbc.car.rivian.values import CAR as RIVIAN -from opendbc.car.tesla.values import CAR as TESLA -from opendbc.car.toyota.values import CAR as TOYOTA -from openpilot.common.constants import CV -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \ - PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES -from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP - -SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState - -ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values()) - -SPEED_LIMITS = { - 'residential': 25 * CV.MPH_TO_MS, # 25 mph - 'city': 35 * CV.MPH_TO_MS, # 35 mph - 'highway': 65 * CV.MPH_TO_MS, # 65 mph - 'freeway': 80 * CV.MPH_TO_MS, # 80 mph -} - -DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2 - - -@pytest.fixture -def car_name(request): - return getattr(request, "param", DEFAULT_CAR) - - -@pytest.fixture(autouse=True) -def set_car_name_on_instance(request, car_name): - instance = getattr(request, "instance", None) - if instance: - instance.car_name = car_name - - -class TestSpeedLimitAssist: - - def setup_method(self, method): - self.params = Params() - self.reset_custom_params() - self.events_sp = EventsSP() - CI = self._setup_platform(self.car_name) - self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP) - self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL) - self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now - self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH - - def teardown_method(self, method): - self.reset_state() - - def _setup_platform(self, car_name): - CarInterface = interfaces[car_name] - CP = CarInterface.get_non_essential_params(car_name) - CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) - CI.CP.openpilotLongitudinalControl = True # always assume it's openpilot longitudinal - sunnypilot_interfaces.setup_interfaces(CI, self.params) - return CI - - def reset_custom_params(self): - self.params.put("IsReleaseSpBranch", True) - self.params.put("SpeedLimitMode", int(Mode.assist)) - self.params.put_bool("IsMetric", False) - self.params.put("SpeedLimitOffsetType", 0) - self.params.put("SpeedLimitValueOffset", 0) - - def reset_state(self): - self.sla.state = SpeedLimitAssistState.disabled - self.sla.frame = -1 - self.sla.last_op_engaged_frame = 0 - self.sla.op_engaged = False - self.sla.op_engaged_prev = False - self.sla._speed_limit = 0. - self.sla.speed_limit_prev = 0. - self.sla.last_valid_speed_limit_offsetted = 0. - self.sla._distance = 0. - self.events_sp.clear() - - def initialize_active_state(self, initialize_v_cruise): - self.sla.state = SpeedLimitAssistState.active - self.sla.v_cruise_cluster = initialize_v_cruise - self.sla.v_cruise_cluster_prev = initialize_v_cruise - self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv) - - def test_initial_state(self): - assert self.sla.state == SpeedLimitAssistState.disabled - assert not self.sla.is_enabled - assert not self.sla.is_active - assert V_CRUISE_UNSET == self.sla.get_v_target_from_control() - - @pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1_GEN1, TESLA.TESLA_MODEL_Y], indirect=True) - def test_disallowed_brands(self, car_name): - """ - Speed Limit Assist is disabled for the following brands and conditions: - - All Tesla and is a release branch; - - All Rivian - """ - assert not self.sla.enabled - - # stay disallowed even when the param may have changed from somewhere else - self.params.put("SpeedLimitMode", int(Mode.assist)) - for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], - SPEED_LIMITS['city'], True, 0, self.events_sp) - assert not self.sla.enabled - - def test_disabled(self): - self.params.put("SpeedLimitMode", int(Mode.off)) - for _ in range(int(10. / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.disabled - - def test_transition_disabled_to_preactive(self): - for _ in range(int(3. / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.preActive - assert self.sla.is_enabled and not self.sla.is_active - - def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self): - for _ in range(int(3. / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.pending - assert self.sla.is_enabled and not self.sla.is_active - - def test_preactive_to_active_with_max_speed_confirmation(self): - self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], - SPEED_LIMITS['highway'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.active - assert self.sla.is_enabled and self.sla.is_active - assert self.sla.output_v_target == SPEED_LIMITS['highway'] - - def test_preactive_timeout_to_inactive(self): - self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - - for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.inactive - - def test_preactive_to_pending_no_speed_limit(self): - self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.pending - assert self.sla.is_enabled and not self.sla.is_active - - def test_pending_to_active_when_speed_limit_available(self): - self.sla.state = SpeedLimitAssistState.pending - self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed - self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) - - self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, - SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.active - - def test_pending_to_adapting_when_below_speed_limit(self): - self.sla.state = SpeedLimitAssistState.pending - self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed - self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) - - self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed, - SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.adapting - assert self.sla.is_enabled and self.sla.is_active - - def test_active_to_adapting_transition(self): - self.initialize_active_state(self.pcm_long_max_set_speed) - - self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], - SPEED_LIMITS['highway'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.adapting - - def test_adapting_to_active_transition(self): - self.sla.state = SpeedLimitAssistState.adapting - self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed - self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) - - self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], - SPEED_LIMITS['highway'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.active - - def test_manual_cruise_change_detection(self): - self.sla.state = SpeedLimitAssistState.active - expected_cruise = SPEED_LIMITS['highway'] - self.sla.v_cruise_cluster_prev = expected_cruise - - different_cruise = SPEED_LIMITS['highway'] + 5 - self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.inactive - - # TODO-SP: test lower CST cases - def test_rapid_speed_limit_changes(self): - self.initialize_active_state(self.pcm_long_max_set_speed) - speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']] - - for _, speed_limit in enumerate(speed_limits): - self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp) - assert self.sla.state in ACTIVE_STATES - - def test_invalid_speed_limits_handling(self): - self.initialize_active_state(self.pcm_long_max_set_speed) - - invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] - - for invalid_limit in invalid_limits: - self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp) - assert isinstance(self.sla.output_v_target, (int, float)) - assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0 - - def test_stale_data_handling(self): - self.initialize_active_state(self.pcm_long_max_set_speed) - old_speed_limit = SPEED_LIMITS['city'] - - self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp) - assert self.sla.state in ACTIVE_STATES - assert self.sla.output_v_target == old_speed_limit - - def test_distance_based_adapting(self): - self.sla.state = SpeedLimitAssistState.adapting - self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed - self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) - - distance = 100.0 - current_speed = SPEED_LIMITS['freeway'] - target_speed = SPEED_LIMITS['highway'] - - self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.adapting - assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions - - def test_long_disengaged_to_disabled(self): - self.initialize_active_state(self.pcm_long_max_set_speed) - - self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], - SPEED_LIMITS['city'], True, 0, self.events_sp) - assert self.sla.state == SpeedLimitAssistState.disabled - assert self.sla.output_v_target == V_CRUISE_UNSET - - def test_maintain_states_with_no_changes(self): - """Test that states are maintained when no significant changes occur""" - test_states = [ - SpeedLimitAssistState.preActive, - SpeedLimitAssistState.pending, - SpeedLimitAssistState.active, - SpeedLimitAssistState.adapting - ] - - for state in test_states: - self.sla.state = state - self.sla.op_engaged = True - - initial_state = state - - self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - - assert self.sla.state in ALL_STATES # Sanity check - - if initial_state == SpeedLimitAssistState.preActive: - assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active] - elif initial_state in ACTIVE_STATES: - assert self.sla.state in ACTIVE_STATES diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py deleted file mode 100644 index c02831d71a..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py +++ /dev/null @@ -1,144 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -import random -import time - -import pytest -from pytest_mock import MockerFixture - -from cereal import custom -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE - -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy - -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source - - -def create_mock(properties, mocker: MockerFixture): - mock = mocker.MagicMock() - for _property, value in properties.items(): - setattr(mock, _property, value) - return mock - - -def setup_sm_mock(mocker: MockerFixture): - cruise_speed_limit = random.uniform(0, 120) - live_map_data_limit = random.uniform(0, 120) - - car_state = create_mock({ - 'gasPressed': False, - 'brakePressed': False, - 'standstill': False, - }, mocker) - car_state_sp = create_mock({ - 'speedLimit': cruise_speed_limit, - }, mocker) - live_map_data = create_mock({ - 'speedLimit': live_map_data_limit, - 'speedLimitValid': True, - 'speedLimitAhead': 0., - 'speedLimitAheadValid': 0., - 'speedLimitAheadDistance': 0., - }, mocker) - gps_data = create_mock({ - 'unixTimestampMillis': time.monotonic() * 1e3, - }, mocker) - sm_mock = mocker.MagicMock() - sm_mock.__getitem__.side_effect = lambda key: { - 'carState': car_state, - 'liveMapDataSP': live_map_data, - 'carStateSP': car_state_sp, - 'gpsLocation': gps_data, - }[key] - return sm_mock - - -parametrized_policies = pytest.mark.parametrize( - "policy, sm_key, function_key", [ - (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car), - (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car), - (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map), - (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map), - ], - ids=lambda val: val.name if hasattr(val, 'name') else str(val) -) - - -@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver]) -class TestSpeedLimitResolverValidation: - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_initial_state(self, resolver_class, policy): - resolver = resolver_class() - resolver.policy = policy - for source in ALL_SOURCES: - if source in resolver.limit_solutions: - assert resolver.limit_solutions[source] == 0. - assert resolver.distance_solutions[source] == 0. - - @parametrized_policies - def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = policy - sm_mock = setup_sm_mock(mocker) - source_speed_limit = sm_mock[sm_key].speedLimit - - # Assert the resolver - resolver.update(source_speed_limit, sm_mock) - assert resolver.speed_limit == source_speed_limit - assert resolver.source == ALL_SOURCES[function_key] - - def test_resolver_combined(self, resolver_class, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = Policy.combined - sm_mock = setup_sm_mock(mocker) - socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map} - minimum_key, minimum_speed_limit = min( - ((key, sm_mock[key].speedLimit) for key in - socket_to_source.keys()), key=lambda x: x[1]) - - # Assert the resolver - resolver.update(minimum_speed_limit, sm_mock) - assert resolver.speed_limit == minimum_speed_limit - assert resolver.source == socket_to_source[minimum_key] - - @parametrized_policies - def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = policy - sm_mock = setup_sm_mock(mocker) - source_speed_limit = sm_mock[sm_key].speedLimit - - # Assert the parsing - resolver.update(source_speed_limit, sm_mock) - assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit - assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0. - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture): - v_ego = 50 - resolver = resolver_class() - resolver.policy = policy - - sm_mock = setup_sm_mock(mocker) - resolver.update(v_ego, sm_mock) - - # After resolution - assert resolver.speed_limit is not None - assert resolver.distance is not None - assert resolver.source is not None - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = policy - sm_mock = mocker.MagicMock() - sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 - resolver._get_from_map_data(sm_mock) - assert resolver.limit_solutions[SpeedLimitSource.map] == 0. - assert resolver.distance_solutions[SpeedLimitSource.map] == 0. diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 4edc0bd470..3980d223ba 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -1,9 +1,7 @@ -import cereal.messaging as messaging from cereal import log, car, custom -from openpilot.common.constants import CV + from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD AlertSize = log.SelfdriveState.AlertSize @@ -18,43 +16,6 @@ EventNameSP = custom.OnroadEventSP.EventName EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()} -def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit - speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) - message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit' - return Alert( - message, - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.) - - -def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH - speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast - speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv) - alert_1_str = "" - alert_2_str = "" - alert_size = AlertSize.none - - if CP.openpilotLongitudinalControl and CP.pcmCruise: - # PCM long - cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric] - pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high - pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) - speed_unit = "km/h" if metric else "mph" - - alert_1_str = "Speed Limit Assist: Activation Required" - alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate" - alert_size = AlertSize.mid - - return Alert( - alert_1_str, - alert_2_str, - AlertStatus.normal, alert_size, - Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1) - - class EventsSP(EventsBase): def __init__(self): super().__init__() @@ -191,34 +152,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), }, - EventNameSP.speedLimitActive: { - ET.WARNING: Alert( - "Automatically adjusting to the posted speed limit", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.), - }, - - EventNameSP.speedLimitChanged: { - ET.WARNING: Alert( - "Set speed changed", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.), - }, - - EventNameSP.speedLimitPreActive: { - ET.WARNING: speed_limit_pre_active_alert, - }, - - EventNameSP.speedLimitPending: { - ET.WARNING: Alert( - "Automatically adjusting to the last speed limit", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.), - }, - EventNameSP.e2eChime: { ET.PERMANENT: Alert( "", diff --git a/third_party/mapd_pfeiferj/README.md b/third_party/mapd_pfeiferj/README.md deleted file mode 100644 index a814f11260..0000000000 --- a/third_party/mapd_pfeiferj/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# MAPD implementation by pfeiferj -https://github.com/pfeiferj/openpilot-mapd/releases/ diff --git a/third_party/mapd_pfeiferj/mapd b/third_party/mapd_pfeiferj/mapd deleted file mode 100755 index c8b50b0791..0000000000 Binary files a/third_party/mapd_pfeiferj/mapd and /dev/null differ