diff --git a/cereal/custom.capnp b/cereal/custom.capnp index f3d59f0ffa..921bb162ad 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -102,9 +102,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { dec @0 :DynamicExperimentalControl; - events @1 :List(OnroadEventSP.Event); - slc @2 :SpeedLimitControl; - struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; enabled @1 :Bool; @@ -115,23 +112,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { blended @1; } } - - struct SpeedLimitControl { - state @0 :SpeedLimitControlState; - enabled @1 :Bool; - active @2 :Bool; - speedLimit @3 :Float32; - speedLimitOffset @4 :Float32; - distToSpeedLimit @5 :Float32; - } - - enum SpeedLimitControlState { - inactive @0; # No speed limit set or not enabled by parameter. - tempInactive @1; # User wants to ignore speed limit until it changes. - preActive @2; - adapting @3; # Reducing speed to match new speed limit. - active @4; # Cruising at speed limit. - } } struct OnroadEventSP @0xda96579883444c35 { @@ -171,10 +151,6 @@ struct OnroadEventSP @0xda96579883444c35 { experimentalModeSwitched @14; wrongCarModeAlertOnly @15; pedalPressedAlertOnly @16; - speedLimitPreActive @17; - speedLimitActive @18; - speedLimitConfirmed @19; - speedLimitValueChange @20; } } @@ -248,7 +224,6 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { } struct CarStateSP @0xb86e6369214c01c8 { - speedLimit @0 :Float32; # m/s } struct LiveMapDataSP @0xf416ec09499d9d19 { @@ -272,13 +247,6 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { lastGpsBearingDeg @17 :Float32; lastGpsAccuracy @18 :Float32; lastGpsBearingAccuracyDeg @19 :Float32; - dataType @20 :DataType; - - enum DataType { - default @0; - offline @1; - online @2; - } } struct CustomReserved9 @0xa1680744031fdb2d { diff --git a/cereal/services.py b/cereal/services.py index 1897679436..3e72041b72 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -84,7 +84,7 @@ _services: dict[str, tuple] = { "carParamsSP": (True, 0.02, 1), "carControlSP": (True, 100., 10), "carStateSP": (True, 100., 10), - "liveMapDataSP": (True, 1.), + "liveMapDataSP": (True, 1., 1), # debug "uiDebug": (True, 0., 1), diff --git a/common/params_keys.h b/common/params_keys.h index e411398455..d2863d7b33 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -177,32 +177,22 @@ inline static std::unordered_map keys = { {"LagdToggle", PERSISTENT | BACKUP}, // mapd - {"MapdVersion", PERSISTENT}, - {"RoadName", CLEAR_ON_ONROAD_TRANSITION}, - {"MapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, {"MapAdvisorySpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, + {"MapdVersion", PERSISTENT}, + {"MapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, {"NextMapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, - {"OSMDownloadBounds", PERSISTENT}, - {"OSMDownloadLocations", PERSISTENT}, - {"OsmDownloadedDate", PERSISTENT}, - {"OsmStateTitle", PERSISTENT}, - {"OsmStateName", PERSISTENT}, - {"OSMDownloadProgress", CLEAR_ON_MANAGER_START}, + {"Offroad_OSMUpdateRequired", CLEAR_ON_MANAGER_START}, {"OsmDbUpdatesCheck", CLEAR_ON_MANAGER_START}, // mapd database update happens with device ON, reset on boot + {"OSMDownloadBounds", PERSISTENT}, + {"OsmDownloadedDate", PERSISTENT}, + {"OSMDownloadLocations", PERSISTENT}, + {"OSMDownloadProgress", CLEAR_ON_MANAGER_START}, {"OsmLocal", PERSISTENT}, {"OsmLocationName", PERSISTENT}, {"OsmLocationTitle", PERSISTENT}, {"OsmLocationUrl", PERSISTENT}, + {"OsmStateName", PERSISTENT}, + {"OsmStateTitle", PERSISTENT}, {"OsmWayTest", PERSISTENT}, - {"Offroad_OSMUpdateRequired", CLEAR_ON_MANAGER_START}, - - // Speed Limit Control - {"SpeedLimitControl", PERSISTENT | BACKUP}, - {"SpeedLimitControlPolicy", PERSISTENT | BACKUP}, - {"SpeedLimitEngageType", PERSISTENT | BACKUP}, - {"SpeedLimitOffsetType", PERSISTENT | BACKUP}, - {"SpeedLimitValueOffset", PERSISTENT | BACKUP}, - {"SpeedLimitWarningType", PERSISTENT | BACKUP}, - {"SpeedLimitWarningOffsetType", PERSISTENT | BACKUP}, - {"SpeedLimitWarningValueOffset", PERSISTENT | BACKUP}, + {"RoadName", CLEAR_ON_ONROAD_TRANSITION}, }; diff --git a/opendbc_repo b/opendbc_repo index bd447a9cec..202a26d943 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit bd447a9cecd999a8dbc8fc51712c81a532bd14d5 +Subproject commit 202a26d94315edaa981ce80ed73c6022d2d66997 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4bb3aecf18..4e842f8d1b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -144,9 +144,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP): clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) - # Get new v_cruise from Speed Limit Control - v_cruise = LongitudinalPlannerSP.update_v_cruise(self, sm, not reset_state, self.v_desired_filter.x, self.a_desired, v_cruise) - if force_slow_decel: v_cruise = 0.0 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index e9892368fb..60036c3082 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -19,8 +19,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', - "liveMapDataSP", "carStateSP"], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2') while True: diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index ec56cbbd3a..3ea3967746 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -88,7 +88,7 @@ class SelfdriveD(CruiseHelper): 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userFlag'] + \ - self.camera_packets + self.sensor_packets + self.gps_packets + ["longitudinalPlanSP"], + self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -196,7 +196,6 @@ class SelfdriveD(CruiseHelper): if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) - self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events) # Add car events, ignore if CAN isn't valid if CS.canValid: diff --git a/sunnypilot/mapd/live_map_data/__init__.py b/sunnypilot/mapd/live_map_data/__init__.py index 8db9022e69..e11c477088 100644 --- a/sunnypilot/mapd/live_map_data/__init__.py +++ b/sunnypilot/mapd/live_map_data/__init__.py @@ -1,11 +1,9 @@ -from cereal import custom 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 -DataType = custom.LiveMapDataSP.DataType 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. diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py index c392a974ab..c95b51c05f 100644 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ b/sunnypilot/mapd/live_map_data/base_map_data.py @@ -1,11 +1,10 @@ import time from abc import abstractmethod, ABC -from cereal import custom, messaging +from cereal import messaging from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.sunnypilot.navd.helpers import Coordinate -from openpilot.sunnypilot.mapd.live_map_data import get_debug class BaseMapData(ABC): @@ -14,12 +13,11 @@ class BaseMapData(ABC): self._last_gps: Coordinate | None = None self._gps_location_service = get_gps_location_service(self.params) self._gps_packets = [self._gps_location_service] - self._data_type = custom.LiveMapDataSP.DataType.default self._sm = messaging.SubMaster(['livePose', 'carControl'] + self._gps_packets) self._pm = messaging.PubMaster(['liveMapDataSP']) @abstractmethod - def update_location(self, current_location: Coordinate): + def update_location(self, current_location: Coordinate | None) -> None: pass @abstractmethod @@ -27,7 +25,7 @@ class BaseMapData(ABC): pass @abstractmethod - def get_next_speed_limit_and_distance(self) -> tuple[float, float]: + def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]: pass @abstractmethod @@ -59,49 +57,34 @@ class BaseMapData(ABC): return result - def get_live_map_data_sp(self, speed_limit, next_speed_limit, next_speed_limit_distance, current_road_name): - last_gps = self.get_current_location() - map_data_msg = messaging.new_message('liveMapDataSP') - map_data_msg.valid = self._is_gps_data_valid() + def publish(self) -> None: + speed_limit = self.get_current_speed_limit() + current_road_name = self.get_current_road_name() + next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance(self._last_gps) - live_map_data = map_data_msg.liveMapDataSP - if last_gps: - live_map_data.lastGpsTimestamp = last_gps.annotations.get('unixTimestampMillis', 0) - live_map_data.lastGpsLatitude = last_gps.latitude - live_map_data.lastGpsLongitude = last_gps.longitude - live_map_data.lastGpsSpeed = last_gps.annotations.get('speed', 0) - live_map_data.lastGpsBearingDeg = last_gps.annotations.get('bearingDeg', 0) - live_map_data.lastGpsAccuracy = last_gps.annotations.get('accuracy', 0) - live_map_data.lastGpsBearingAccuracyDeg = last_gps.annotations.get('bearingAccuracyDeg', 0) + mapd_sp_send = messaging.new_message('liveMapDataSP') + mapd_sp_send.valid = self._is_gps_data_valid() + live_map_data = mapd_sp_send.liveMapDataSP + + if self._last_gps: + live_map_data.lastGpsTimestamp = self._last_gps.annotations.get('unixTimestampMillis', 0) + live_map_data.lastGpsLatitude = self._last_gps.latitude + live_map_data.lastGpsLongitude = self._last_gps.longitude + live_map_data.lastGpsSpeed = self._last_gps.annotations.get('speed', 0) + live_map_data.lastGpsBearingDeg = self._last_gps.annotations.get('bearingDeg', 0) + live_map_data.lastGpsAccuracy = self._last_gps.annotations.get('accuracy', 0) + live_map_data.lastGpsBearingAccuracyDeg = self._last_gps.annotations.get('bearingAccuracyDeg', 0) live_map_data.speedLimitValid = bool(speed_limit > 0) live_map_data.speedLimit = speed_limit live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0) - live_map_data.speedLimitAhead = float(next_speed_limit) - live_map_data.speedLimitAheadDistance = float(next_speed_limit_distance) - live_map_data.currentRoadName = str(current_road_name) - live_map_data.dataType = self._data_type + live_map_data.speedLimitAhead = next_speed_limit + live_map_data.speedLimitAheadDistance = next_speed_limit_distance + live_map_data.currentRoadName = current_road_name - return map_data_msg + self._pm.send('liveMapDataSP', mapd_sp_send) - def publish(self): - speed_limit = self.get_current_speed_limit() - current_road_name = self.get_current_road_name() - next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() - - live_map_data_sp = self.get_live_map_data_sp( - speed_limit, - next_speed_limit, - next_speed_limit_distance, - current_road_name - ) - - self._pm.send('liveMapDataSP', live_map_data_sp) - get_debug(f"SRC: [{self.__class__.__name__}] | SLC: [{speed_limit}] | NSL: [{next_speed_limit}] | " + - f"NSLD: [{next_speed_limit_distance}] | CRN: [{current_road_name}] | GPS: [{self._last_gps}] " + - f"Annotations: [{', '.join(f'{key}: {value}' for key, value in self._last_gps.annotations.items()) if self._last_gps else []}]") - - def tick(self): + def tick(self) -> None: self._sm.update() self._last_gps = self.get_current_location() self.update_location(self._last_gps) diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py index 44527d56be..8209e7b9c0 100644 --- a/sunnypilot/mapd/live_map_data/debug.py +++ b/sunnypilot/mapd/live_map_data/debug.py @@ -9,11 +9,10 @@ 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 set_core_affinity +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 -from openpilot.common.swaglog import cloudlog def excepthook(args): @@ -22,10 +21,7 @@ def excepthook(args): def live_map_data_sp_thread(): - try: - set_core_affinity([0, 1, 2, 3]) - except Exception: - cloudlog.exception("mapd: failed to set core affinity") + config_realtime_process([0, 1, 2, 3], 5) params = Params() gps_location_service = get_gps_location_service(params) diff --git a/sunnypilot/mapd/live_map_data/osm_map_data.py b/sunnypilot/mapd/live_map_data/osm_map_data.py index 3e2ec6823c..8ea85b792b 100644 --- a/sunnypilot/mapd/live_map_data/osm_map_data.py +++ b/sunnypilot/mapd/live_map_data/osm_map_data.py @@ -3,39 +3,33 @@ import platform from openpilot.common.params import Params from openpilot.sunnypilot.navd.helpers import Coordinate -from openpilot.sunnypilot.mapd.live_map_data import DataType from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData class OsmMapData(BaseMapData): def __init__(self): super().__init__() - self.last_gps = Coordinate(0.0, 0.0) self.params = Params() self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params - self.data_type = DataType.offline - def update_location(self, current_location): - self.last_gps = current_location - if not self.last_gps: + def update_location(self, current_location: Coordinate | None) -> None: + if not current_location: return last_gps_position_for_osm = { - "latitude": self.last_gps.latitude, - "longitude": self.last_gps.longitude, - "bearing": self.last_gps.annotations.get("bearingDeg", 0) + "latitude": current_location.latitude, + "longitude": current_location.longitude, + "bearing": current_location.annotations.get("bearingDeg", 0) } self.mem_params.put("LastGPSPosition", json.dumps(last_gps_position_for_osm)) - def get_current_speed_limit(self): - speed_limit = self.mem_params.get("MapSpeedLimit", encoding='utf8') - return float(speed_limit) if speed_limit else 0.0 + def get_current_speed_limit(self) -> float: + return float(self.mem_params.get("MapSpeedLimit", encoding='utf8') or 0.0) - def get_current_road_name(self): - current_road_name = self.mem_params.get("RoadName", encoding='utf8') - return current_road_name if current_road_name else "" + def get_current_road_name(self) -> str: + return self.mem_params.get("RoadName", encoding='utf8') or "" - def get_next_speed_limit_and_distance(self) -> tuple[float, float]: + def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]: next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit", encoding='utf8') next_speed_limit_section = json.loads(next_speed_limit_section_str) if next_speed_limit_section_str else {} next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0) @@ -45,6 +39,6 @@ class OsmMapData(BaseMapData): 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_gps or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates) + next_speed_limit_distance = (current_location 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 index d2bf6914a1..a9347a2b29 100644 --- a/sunnypilot/mapd/live_map_data/standalone.py +++ b/sunnypilot/mapd/live_map_data/standalone.py @@ -6,10 +6,9 @@ import threading import traceback -from openpilot.common.realtime import Ratekeeper, set_core_affinity +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 -from openpilot.common.swaglog import cloudlog def excepthook(args): @@ -18,10 +17,7 @@ def excepthook(args): def live_map_data_sp_thread(): - try: - set_core_affinity([0, 1, 2, 3]) - except Exception: - cloudlog.exception("mapd: failed to set core affinity") + config_realtime_process([0, 1, 2, 3], 5) live_map_sp = OsmMapData() rk = Ratekeeper(1, print_delay_threshold=None) diff --git a/sunnypilot/mapd/mapd_installer.py b/sunnypilot/mapd/mapd_installer.py index f91da376b3..c2d00d7ecf 100755 --- a/sunnypilot/mapd/mapd_installer.py +++ b/sunnypilot/mapd/mapd_installer.py @@ -7,12 +7,14 @@ import traceback import requests from pathlib import Path from urllib.request import urlopen -import openpilot.system.sentry as sentry + from cereal import messaging from openpilot.common.params import Params +from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH, MAPD_BIN_DIR +from openpilot.system.hardware.hw import Paths from openpilot.system.ui.spinner import Spinner -from openpilot.sunnypilot.mapd.mapd_manager import COMMON_DIR, MAPD_PATH, MAPD_BIN_DIR from openpilot.system.version import is_prebuilt +import openpilot.system.sentry as sentry VERSION = 'v1.9.0' URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd" @@ -22,28 +24,28 @@ class MapdInstallManager: def __init__(self, spinner_ref: Spinner): self._spinner = spinner_ref - def download(self): + def download(self) -> None: self.ensure_directories_exist() self._download_file() self.update_installed_version(VERSION) - def check_and_download(self): + def check_and_download(self) -> None: if self.download_needed(): self.download() @staticmethod - def download_needed(): + def download_needed() -> bool: return not os.path.exists(MAPD_PATH) or MapdInstallManager.get_installed_version() != VERSION @staticmethod - def ensure_directories_exist(): - if not os.path.exists(COMMON_DIR): - os.makedirs(COMMON_DIR) + 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, content): + def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None: with open(file_path, 'wb') as output: output.write(content) output.flush() @@ -51,7 +53,7 @@ class MapdInstallManager: 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): + def _download_file(self, num_retries=5) -> None: temp_file = Path(MAPD_PATH + ".tmp") download_timeout = 60 for cnt in range(num_retries): @@ -75,14 +77,14 @@ class MapdInstallManager: logging.error("Failed to download file after all retries") @staticmethod - def update_installed_version(version): + def update_installed_version(version: str) -> None: Params().put("MapdVersion", version) @staticmethod - def get_installed_version(): + def get_installed_version() -> str | None: return Params().get("MapdVersion", encoding="utf-8") - def wait_for_internet_connection(self, return_on_failure=False): + def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool | None: max_retries = 10 for retries in range(max_retries + 1): self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]") @@ -95,7 +97,7 @@ class MapdInstallManager: if return_on_failure and retries == max_retries: return False - def non_prebuilt_install(self): + def non_prebuilt_install(self) -> bool | None: sm = messaging.SubMaster(['deviceState']) metered = sm['deviceState'].networkMetered diff --git a/sunnypilot/mapd/mapd_manager.py b/sunnypilot/mapd/mapd_manager.py index afd632cff7..7fa3509600 100644 --- a/sunnypilot/mapd/mapd_manager.py +++ b/sunnypilot/mapd/mapd_manager.py @@ -1,38 +1,31 @@ import json import time import platform - -import numpy as np - -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.sunnypilot.navd.helpers import Coordinate -from openpilot.sunnypilot.mapd.live_map_data import QUERY_RADIUS -from openpilot.common.realtime import Ratekeeper, set_core_affinity -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware.hw import Paths import os import glob import shutil +from openpilot.common.basedir import BASEDIR +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.sunnypilot.mapd.live_map_data import R +from openpilot.system.hardware.hw import Paths # PFEIFER - MAPD {{ params = Params() mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params # }} PFEIFER - MAPD -COMMON_DIR = Paths.mapd_root() MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj') MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd') -def get_files_for_cleanup(): +def get_files_for_cleanup() -> list[str]: paths = [ - f"{COMMON_DIR}/db", - f"{COMMON_DIR}/v*" + f"{Paths.mapd_root()}/db", + f"{Paths.mapd_root()}/v*" ] files_to_remove = [] for path in paths: @@ -45,7 +38,7 @@ def get_files_for_cleanup(): return files_to_remove -def cleanup_OLD_OSM_data(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]): @@ -57,29 +50,6 @@ def cleanup_OLD_OSM_data(files_to_remove): shutil.rmtree(file, ignore_errors=False) -def _get_current_bounding_box(self, radius: float): - self.last_query_radius = radius - # Calculate the bounding box coordinates for the bbox containing the circle around location. - bbox_angle = float(np.degrees(radius / R)) - - lat = float(self.last_gps.latitude) - lon = float(self.last_gps.longitude) - - return { - "min_lat": lat - bbox_angle, - "min_lon": lon - bbox_angle, - "max_lat": lat + bbox_angle, - "max_lon": lon + bbox_angle, - } - - -def _request_refresh_osm_bounds_data(self): - self.last_refresh_loc = Coordinate(self.last_gps.latitude, self.last_gps.longitude) - self.last_query_radius = QUERY_RADIUS - current_bounding_box = self._get_current_bounding_box(self.last_query_radius) - mem_params.put("OSMDownloadBounds", json.dumps(current_bounding_box)) - - def request_refresh_osm_location_data(nations: list[str], states: list[str] = None) -> None: params.put("OsmDownloadedDate", str(time.time())) params.put_bool("OsmDbUpdatesCheck", False) @@ -93,7 +63,7 @@ def request_refresh_osm_location_data(nations: list[str], states: list[str] = No mem_params.put("OSMDownloadLocations", osm_download_locations) -def filter_nations_and_states(nations: list[str], states: list[str] = None): +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. @@ -122,11 +92,11 @@ def filter_nations_and_states(nations: list[str], states: list[str] = None): return nations, states or [] -def update_osm_db(): +def update_osm_db() -> None: # last_downloaded_date = float(params.get('OsmDownloadedDate', encoding='utf-8') or 0.0) # if params.get_bool("OsmDbUpdatesCheck") or time.time() - last_downloaded_date >= 604800: # 7 days * 24 hours/day * 60 if params.get_bool("OsmDbUpdatesCheck"): - cleanup_OLD_OSM_data(get_files_for_cleanup()) + cleanup_old_osm_data(get_files_for_cleanup()) country = params.get('OsmLocationName', encoding='utf-8') state = params.get('OsmStateName', encoding='utf-8') or "All" filtered_nations, filtered_states = filter_nations_and_states([country], [state]) @@ -139,21 +109,19 @@ def update_osm_db(): mem_params.put("LastGPSPosition", "{}") -def main_thread(sm=None, pm=None): - try: - set_core_affinity([0, 1, 2, 3]) - except Exception: - cloudlog.exception("mapd: failed to set core affinity") +def main_thread(): + 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(COMMON_DIR) + os.mkdir(Paths.mapd_root()) except FileExistsError: pass except PermissionError: - cloudlog.exception(f"mapd: failed to make {COMMON_DIR}") + cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}") while True: show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal") diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f2812c44bf..56f32373d5 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,20 +7,14 @@ See the LICENSE.md file in the root directory for more details. from cereal import messaging, custom from opendbc.car import structs -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController -from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): - self.events_sp = EventsSP() - self.dec = DynamicExperimentalController(CP, mpc) - self.slc = SpeedLimitController(CP) def get_mpc_mode(self) -> str | None: if not self.dec.active(): @@ -28,17 +22,6 @@ class LongitudinalPlannerSP: return self.dec.mode() - def update_v_cruise(self, sm: messaging.SubMaster, long_enabled: bool, v_ego: float, a_ego: float, v_cruise: float) -> float: - self.events_sp.clear() - - self.slc.update(long_enabled, v_ego, a_ego, sm, v_cruise, self.events_sp) - - v_cruise_slc = self.slc.speed_limit_offseted if self.slc.is_active else V_CRUISE_UNSET - - v_cruise_final = min(v_cruise, v_cruise_slc) - - return v_cruise_final - def update(self, sm: messaging.SubMaster) -> None: self.dec.update(sm) @@ -48,7 +31,6 @@ class LongitudinalPlannerSP: plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) longitudinalPlanSP = plan_sp_send.longitudinalPlanSP - longitudinalPlanSP.events = self.events_sp.to_msg() # Dynamic Experimental Control dec = longitudinalPlanSP.dec @@ -56,13 +38,4 @@ class LongitudinalPlannerSP: dec.enabled = self.dec.enabled() dec.active = self.dec.active() - # Speed Limit Control - slc = longitudinalPlanSP.slc - slc.state = self.slc.state - slc.enabled = self.slc.is_enabled - slc.active = self.slc.is_active - slc.speedLimit = float(self.slc.speed_limit) - slc.speedLimitOffset = float(self.slc.speed_limit_offset) - slc.distToSpeedLimit = float(self.slc.distance) - pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py deleted file mode 100644 index b0a6675491..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -from cereal import custom - -DEBUG = True -PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates. -TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal. - -# Lookup table for speed limit percent offset depending on speed. -LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h -LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h - -# Constants for Limit controllers. -LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. -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. -LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. - -SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py deleted file mode 100644 index 4656ee286b..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py +++ /dev/null @@ -1,26 +0,0 @@ -from enum import IntEnum - - -class Source(IntEnum): - none = 0 - car_state = 1 - map_data = 2 - - -class Policy(IntEnum): - map_data_only = 0 - car_state_only = 1 - map_data_priority = 2 - car_state_priority = 3 - combined = 4 - - -class Engage(IntEnum): - auto = 0 - user_confirm = 1 - - -class OffsetType(IntEnum): - default = 0 - fixed = 1 - percentage = 2 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py deleted file mode 100644 index b15c63bf05..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py +++ /dev/null @@ -1,19 +0,0 @@ -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import DEBUG, SpeedLimitControlState -from openpilot.common.swaglog import cloudlog - - -def debug(msg): - if not DEBUG: - return - cloudlog.debug(msg) - - -def description_for_state(speed_limit_control_state): - if speed_limit_control_state == SpeedLimitControlState.inactive: - return 'INACTIVE' - if speed_limit_control_state == SpeedLimitControlState.tempInactive: - return 'TEMP_INACTIVE' - if speed_limit_control_state == SpeedLimitControlState.adapting: - return 'ADAPTING' - if speed_limit_control_state == SpeedLimitControlState.active: - return 'ACTIVE' diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py deleted file mode 100644 index 9a28611259..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ /dev/null @@ -1,344 +0,0 @@ -from collections.abc import Callable -import numpy as np -import time - -from cereal import messaging, custom -from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \ - PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver -from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP -from openpilot.selfdrive.modeld.constants import ModelConstants - -EventNameSP = custom.OnroadEventSP.EventName - -ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting) -ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.tempInactive, *ACTIVE_STATES) - - -class SpeedLimitController: - _speed_limit: float - _distance: float - _source: Source - _v_ego: float - _a_ego: float - _v_offset: float - - def __init__(self, CP): - self._params = Params() - self._CP = CP - self._policy = self._read_policy_param() - self._resolver = SpeedLimitResolver(self._policy) - self._last_params_update = 0.0 - self._last_op_engaged_time = 0.0 - self._is_metric = self._params.get_bool("IsMetric") - self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._op_engaged = False - self._op_engaged_prev = False - self._v_ego = 0. - self._a_ego = 0. - self._v_offset = 0. - self._v_cruise_setpoint = 0. - self._v_cruise_setpoint_prev = 0. - self._v_cruise_setpoint_changed = False - self._speed_limit = 0. - self._speed_limit_prev = 0. - self._speed_limit_changed = False - self._distance = 0. - self._source = Source.none - self._state = SpeedLimitControlState.inactive - self._state_prev = SpeedLimitControlState.inactive - self._gas_pressed = False - self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise - - self._offset_type = OffsetType(self._read_int_param("SpeedLimitOffsetType")) - self._offset_value = self._read_int_param("SpeedLimitValueOffset") - self._warning_type = self._read_int_param("SpeedLimitWarningType") - self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType")) - self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") - self._engage_type = self._read_engage_type_param() - self._current_time = 0. - self._v_cruise_rounded = 0. - self._v_cruise_prev_rounded = 0. - self._speed_limit_offsetted_rounded = 0. - self._speed_limit_warning_offsetted_rounded = 0. - self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH - - # Mapping functions to state transitions - self.state_transition_strategy = { - # Transition functions for each state - SpeedLimitControlState.inactive: self.transition_state_from_inactive, - SpeedLimitControlState.tempInactive: self.transition_state_from_temp_inactive, - SpeedLimitControlState.adapting: self.transition_state_from_adapting, - SpeedLimitControlState.active: self.transition_state_from_active, - SpeedLimitControlState.preActive: self.transition_state_from_pre_active, - } - - # FIXME-SP: unused? - # Solution functions mapped to respective states - self.acceleration_solutions = { - # Solution functions for each state - SpeedLimitControlState.tempInactive: self.get_current_acceleration_as_target, - SpeedLimitControlState.inactive: self.get_current_acceleration_as_target, - SpeedLimitControlState.adapting: self.get_adapting_state_target_acceleration, - SpeedLimitControlState.active: self.get_active_state_target_acceleration, - SpeedLimitControlState.preActive: self.get_current_acceleration_as_target, - } - - @property - def state(self) -> SpeedLimitControlState: - return self._state - - @state.setter - def state(self, value) -> None: - if value != self._state: - debug(f'Speed Limit Controller state: {description_for_state(value)}') - - if value == SpeedLimitControlState.tempInactive: - # Reset previous speed limit to current value as to prevent going out of tempInactive in - # a single cycle when the speed limit changes at the same time the user has temporarily deactivated it. - self._speed_limit_prev = self._speed_limit - - self._state = value - - @property - def is_enabled(self) -> bool: - return self.state in ENABLED_STATES and self._is_enabled - - @property - def is_active(self) -> bool: - return self.state in ACTIVE_STATES and self._is_enabled - - @property - def speed_limit_offseted(self) -> float: - return self._speed_limit + self.speed_limit_offset - - @property - def speed_limit_offset(self) -> float: - return self._get_offset(self._offset_type, self._offset_value) - - @property - def speed_limit_warning_offset(self) -> float: - return self._get_offset(self._warning_offset_type, self._warning_offset_value) - - @property - def speed_limit(self) -> float: - return self._speed_limit - - @property - def distance(self) -> float: - return self._distance - - @property - def source(self) -> Source: - return self._source - - def _get_offset(self, offset_type: OffsetType, offset_value: int) -> float: - if offset_type == OffsetType.default: - return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) - elif offset_type == OffsetType.fixed: - return offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS) - elif offset_type == OffsetType.percentage: - return offset_value * 0.01 * self._speed_limit - else: - raise NotImplementedError("Offset not supported") - - def _update_v_cruise_setpoint_prev(self) -> None: - self._v_cruise_setpoint_prev = self._v_cruise_setpoint - - def _update_params(self) -> None: - if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD: - self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._offset_type = OffsetType(self._read_int_param("SpeedLimitOffsetType")) - self._offset_value = self._read_int_param("SpeedLimitValueOffset") - self._warning_type = self._read_int_param("SpeedLimitWarningType") - self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType")) - self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") - self._policy = self._read_policy_param() - self._is_metric = self._params.get_bool("IsMetric") - self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH - self._resolver.change_policy(self._policy) - self._engage_type = self._read_engage_type_param() - - self._last_params_update = self._current_time - - def _read_policy_param(self) -> Policy: - try: - return Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8'))) - except (ValueError, TypeError): - return Policy.car_state_priority - - def _read_engage_type_param(self) -> Engage: - if self._pcm_cruise_op_long: - return Engage.auto - - return Engage(self._read_int_param("SpeedLimitEngageType", validator=lambda x: np.clip(x, Engage.auto, Engage.user_confirm))) - - def _read_int_param(self, key: str, default: int = 0, validator: Callable[[int], int] = None) -> int: - try: - val = int(self._params.get(key, encoding='utf8')) - - if validator is not None: - return validator(val) - return val - except (ValueError, TypeError): - return default - - def _update_calculations(self) -> None: - # Update current velocity offset (error) - self._v_offset = self.speed_limit_offseted - self._v_ego - - # Track the time op becomes active to prevent going to tempInactive right away after - # op enabling since controlsd will change the cruise speed every time on enabling and this will - # cause a temp inactive transition if the controller is updated before controlsd sets actual cruise - # speed. - if not self._op_engaged_prev and self._op_engaged: - self._last_op_engaged_time = self._current_time - - # Update change tracking variables - self._speed_limit_changed = self._speed_limit != self._speed_limit_prev - self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev - self._speed_limit_prev = self._speed_limit - if self._engage_type != Engage.user_confirm: - self._update_v_cruise_setpoint_prev() - self._op_engaged_prev = self._op_engaged - - self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor)) - self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._speed_factor)) - self._speed_limit_offsetted_rounded = 0 if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._speed_factor)) - self._speed_limit_warning_offsetted_rounded = 0 if self._speed_limit == 0 else \ - int(round((self._speed_limit + self.speed_limit_warning_offset) * self._speed_factor)) - - def transition_state_from_inactive(self) -> None: - """ Make state transition from inactive state """ - if self._engage_type == Engage.user_confirm: - if (((self._last_op_engaged_time + 7.) >= self._current_time >= (self._last_op_engaged_time + 2.)) or - self._speed_limit_changed): - if self._speed_limit_changed: - self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation - self.state = SpeedLimitControlState.preActive - elif self._engage_type == Engage.auto: - if self._v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting - else: - self.state = SpeedLimitControlState.active - - def transition_state_from_temp_inactive(self) -> None: - """ Make state transition from temporary inactive state """ - if self._speed_limit_changed: - if self._engage_type == Engage.user_confirm: - self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation - self.state = SpeedLimitControlState.preActive - elif self._engage_type == Engage.auto: - self.state = SpeedLimitControlState.inactive - - def transition_state_from_pre_active(self) -> None: - """ Make state transition from preActive state """ - if self._current_time >= (self._last_op_engaged_time + 7.): - self.state = SpeedLimitControlState.inactive - elif (self._last_op_engaged_time + 7.) > self._current_time > (self._last_op_engaged_time + 2.): - if self._speed_limit_changed: - self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation - elif self._v_cruise_prev_rounded < self._speed_limit_offsetted_rounded: - if self._v_cruise_setpoint > self._v_cruise_setpoint_prev: - self.state = SpeedLimitControlState.active - elif self._v_cruise_prev_rounded > self._speed_limit_offsetted_rounded: - if self._v_cruise_setpoint < self._v_cruise_setpoint_prev: - self.state = SpeedLimitControlState.active - elif self._v_cruise_prev_rounded == self._speed_limit_offsetted_rounded: - self.state = SpeedLimitControlState.active - - def transition_state_from_adapting(self) -> None: - """ Make state transition from adapting state """ - if self._v_offset >= LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.active - - def transition_state_from_active(self) -> None: - """ Make state transition from active state """ - if self._engage_type == Engage.user_confirm: - if self._state_prev == SpeedLimitControlState.active: - if self._v_cruise_setpoint_changed and self._v_cruise_rounded != self._speed_limit_offsetted_rounded: - self.state = SpeedLimitControlState.tempInactive - elif self._speed_limit_changed: - self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation - self.state = SpeedLimitControlState.preActive - elif self._engage_type == Engage.auto: - if self._v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting - - def _state_transition(self) -> None: - self._state_prev = self._state - - # In any case, if op is disabled, or speed limit control is disabled - # or the reported speed limit is 0 or gas is pressed, deactivate. - if not self._op_engaged or not self._is_enabled or self._speed_limit == 0: - self.state = SpeedLimitControlState.inactive - return - - # In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed. - # Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations - # due to controlsd logic changing cruise setpoint when going active. - if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \ - self._current_time > (self._last_op_engaged_time + TEMP_INACTIVE_GUARD_PERIOD): - self.state = SpeedLimitControlState.tempInactive - return - - self.state_transition_strategy[self.state]() - - if self._engage_type == Engage.user_confirm: - self._update_v_cruise_setpoint_prev() - - def get_current_acceleration_as_target(self) -> float: - """ When state is inactive or tempInactive, preserve current acceleration """ - return self._a_ego - - def get_adapting_state_target_acceleration(self) -> float: - """ In adapting state, calculate target acceleration based on speed limit and current velocity """ - if self.distance > 0: - return (self.speed_limit_offseted ** 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: - """ In active state, aim to keep speed constant around control time horizon """ - return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) - - def _update_events(self, events_sp: EventsSP) -> None: - if self._speed_limit > 0 and self._warning_type == 2 and \ - self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._speed_factor)): - events_sp.add(EventNameSP.speedLimitPreActive) - - if not self.is_active: - if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \ - self._v_cruise_rounded != self._speed_limit_offsetted_rounded: - events_sp.add(EventNameSP.speedLimitPreActive) - else: - if self._engage_type == Engage.user_confirm: - if self._state_prev == SpeedLimitControlState.preActive: - events_sp.add(EventNameSP.speedLimitConfirmed) - events_sp.add(EventNameSP.speedLimitActive) - elif self._engage_type == Engage.auto: - if self._state_prev not in ACTIVE_STATES: - events_sp.add(EventNameSP.speedLimitActive) - elif self._speed_limit_changed != 0: - events_sp.add(EventNameSP.speedLimitValueChange) - - def update(self, enabled: bool, v_ego: float, a_ego: float, sm: messaging.SubMaster, v_cruise_setpoint: float, events_sp: EventsSP) -> None: - _car_state = sm['carState'] - self._op_engaged = enabled and self._CP.openpilotLongitudinalControl - self._v_ego = v_ego - self._a_ego = a_ego - self._v_cruise_setpoint = v_cruise_setpoint - self._gas_pressed = _car_state.gasPressed - self._current_time = time.monotonic() - - self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm) - - self._update_params() - self._update_calculations() - self._state_transition() - self._update_events(events_sp) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py deleted file mode 100644 index cae5f2ea9c..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ /dev/null @@ -1,125 +0,0 @@ -import time -import numpy as np - -from cereal import messaging, custom - -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug - - -class SpeedLimitResolver: - _sm: messaging.SubMaster - _limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources - _distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources - _v_ego: float - _current_speed_limit: float - - def __init__(self, policy: Policy): - self._limit_solutions = {} - self._distance_solutions = {} - - self._policy = policy - self._policy_to_sources_map = { - Policy.car_state_only: [Source.car_state], - Policy.car_state_priority: [Source.car_state, Source.map_data], - Policy.map_data_priority: [Source.map_data, Source.car_state], - Policy.map_data_only: [Source.map_data], - Policy.combined: [Source.car_state, Source.map_data], - } - for source in Source: - self._reset_limit_sources(source) - - def change_policy(self, policy: Policy) -> None: - self._policy = policy - - def _reset_limit_sources(self, source: Source) -> None: - self._limit_solutions[source] = 0. - self._distance_solutions[source] = 0. - - def _is_sock_updated(self, sock: str) -> bool: - return self._sm.alive[sock] and self._sm.updated[sock] - - def resolve(self, v_ego: float, current_speed_limit: float, sm: messaging.SubMaster) -> tuple[float, float, Source]: - self._v_ego = v_ego - self._current_speed_limit = current_speed_limit - self._sm = sm - - self._resolve_limit_sources() - return self._consolidate() - - def _resolve_limit_sources(self) -> None: - """Get limit solutions from each data source""" - self._get_from_car_state() - self._get_from_map_data() - - def _get_from_car_state(self) -> None: - if not self._is_sock_updated('carStateSP'): - debug('SL: No carStateSP instruction for speed limit') - return - - self._reset_limit_sources(Source.car_state) - self._limit_solutions[Source.car_state] = self._sm['carStateSP'].speedLimit - self._distance_solutions[Source.car_state] = 0. - - def _get_from_map_data(self) -> None: - if not self._is_sock_updated("liveMapDataSP"): - debug('SL: No map data for speed limit') - return - - # Load limits from map_data - self._reset_limit_sources(Source.map_data) - self._process_map_data(self._sm["liveMapDataSP"]) - - def _process_map_data(self, map_data: custom.LiveMapDataSP) -> None: - gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 - if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: - debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_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(speed_limit, next_speed_limit, map_data) - - def _calculate_map_data_limits(self, speed_limit: float, next_speed_limit: float, map_data: custom.LiveMapDataSP) -> None: - distance_since_fix = self._v_ego * (time.time() - map_data.lastGpsTimestamp * 1e-3) - distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) - - self._limit_solutions[Source.map_data] = speed_limit - self._distance_solutions[Source.map_data] = 0. - - 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[Source.map_data] = next_speed_limit - self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead - - def _consolidate(self) -> tuple[float, float, Source]: - source = self._get_source_solution_according_to_policy() - self.speed_limit = self._limit_solutions[source] if source else 0. - self.distance = self._distance_solutions[source] if source else 0. - self.source = source or Source.none - - debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}') - return self.speed_limit, self.distance, self.source - - def _get_source_solution_according_to_policy(self) -> Source | None: - 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(source) - - limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float) - sources = np.array([source.value for source in sources_for_policy], dtype=int) - - if len(limits) > 0: - min_idx = np.argmin(limits) - return Source(sources[min_idx]) - - return None diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py deleted file mode 100644 index 972f70d326..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py +++ /dev/null @@ -1,78 +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 -from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP - -EventNameSP = custom.OnroadEventSP.EventName -State = custom.LongitudinalPlanSP.SpeedLimitControlState - -ACTIVE_STATES = (State.active, State.adapting) -ENABLED_STATES = (State.preActive, State.tempInactive, *ACTIVE_STATES) - - -class StateMachine: - def __init__(self): - self.state = State.inactive - - def update(self, events_sp: EventsSP) -> tuple[bool, bool]: - # INACTIVE - if self.state == State.inactive: - if events_sp.has(EventNameSP.speedLimitEnable): - self.state = State.preActive - elif events_sp.has(EventNameSP.speedLimitAdapting): - self.state = State.adapting - elif events_sp.has(EventNameSP.speedLimitActive): - self.state = State.active - - # PRE ACTIVE - elif self.state == State.preActive: - if events_sp.has(EventNameSP.speedLimitDisable): - self.state = State.inactive - elif events_sp.has(EventNameSP.speedLimitUserCancel): - self.state = State.inactive - elif events_sp.has(EventNameSP.speedLimitUserConfirm): - self.state = State.active - elif events_sp.has(EventNameSP.speedLimitActive): - self.state = State.active - - # ACTIVE - elif self.state == State.active: - if events_sp.has(EventNameSP.speedLimitDisable): - self.state = State.inactive - elif events_sp.has(EventNameSP.speedLimitUserCancel): - self.state = State.tempInactive - elif events_sp.has(EventNameSP.speedLimitAdapting): - self.state = State.adapting - elif events_sp.has(EventNameSP.speedLimitValueChange): - # For user confirm mode, transition to preActive on speed limit change - if events_sp.has(EventNameSP.speedLimitEnable): - self.state = State.preActive - - # ADAPTING - elif self.state == State.adapting: - if events_sp.has(EventNameSP.speedLimitDisable): - self.state = State.inactive - elif events_sp.has(EventNameSP.speedLimitUserCancel): - self.state = State.tempInactive - elif events_sp.has(EventNameSP.speedLimitReached): - self.state = State.active - - # TEMP INACTIVE - elif self.state == State.tempInactive: - if events_sp.has(EventNameSP.speedLimitDisable): - self.state = State.inactive - elif events_sp.has(EventNameSP.speedLimitValueChange): - # When speed limit changes, reactivate - if events_sp.has(EventNameSP.speedLimitEnable): - self.state = State.preActive - else: - self.state = State.inactive - - enabled = self.state in ENABLED_STATES - active = self.state in ACTIVE_STATES - - return enabled, active diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py deleted file mode 100644 index 0ae8dff8fc..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py +++ /dev/null @@ -1,128 +0,0 @@ -import random -import time - -import pytest -from pytest_mock import MockerFixture - -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE - -# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy - - -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., - 'lastGpsTimestamp': time.time() * 1e3, - }, mocker) - sm_mock = mocker.MagicMock() - sm_mock.__getitem__.side_effect = lambda key: { - 'carState': car_state, - 'liveMapDataSP': live_map_data, - 'carStateSP': car_state_sp, - }[key] - return sm_mock - - -parametrized_policies = pytest.mark.parametrize( - "policy, sm_key, function_key", [ - (Policy.car_state_only, 'carStateSP', 'car_state'), - (Policy.car_state_priority, 'carStateSP', 'car_state'), - (Policy.map_data_only, 'liveMapDataSP', 'map_data'), - (Policy.map_data_priority, 'liveMapDataSP', 'map_data'), - ], - ids=lambda val: val.name if hasattr(val, 'name') else str(val) -) - - -@pytest.mark.parametrize("resolver_class", [RefactoredSpeedLimitResolver], ids=["Refactored"]) -class TestSpeedLimitResolverValidation: - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_initial_state(self, resolver_class, policy): - resolver = resolver_class(policy) - for source in Source: - 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(policy) - sm_mock = setup_sm_mock(mocker) - source_speed_limit = sm_mock[sm_key].speedLimit - - # Assert the resolver - speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock) - assert speed_limit == source_speed_limit - assert source == Source[function_key] - - def test_resolver_combined(self, resolver_class, mocker: MockerFixture): - resolver = resolver_class(Policy.combined) - sm_mock = setup_sm_mock(mocker) - socket_to_source = {'carStateSP': Source.car_state, 'liveMapDataSP': Source.map_data} - 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 - speed_limit, _, source = resolver.resolve(minimum_speed_limit, 0, sm_mock) - assert speed_limit == minimum_speed_limit - assert source == socket_to_source[minimum_key] - - @parametrized_policies - def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class(policy) - sm_mock = setup_sm_mock(mocker) - source_speed_limit = sm_mock[sm_key].speedLimit - - # Assert the parsing - speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock) - assert resolver._limit_solutions[Source[function_key]] == source_speed_limit - assert resolver._distance_solutions[Source[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(policy) - - sm_mock = setup_sm_mock(mocker) - _speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock) - - # After resolution - assert _speed_limit is not None - assert _distance is not None - assert _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(policy) - sm_mock = mocker.MagicMock() - sm_mock['liveMapDataSP'].lastGpsTimestamp = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 - resolver._sm = sm_mock - resolver._get_from_map_data() - assert resolver._limit_solutions[Source.map_data] == 0. - assert resolver._distance_solutions[Source.map_data] == 0. diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 5b76e035be..d81345a14e 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -1,6 +1,4 @@ -import cereal.messaging as messaging from cereal import log, car, custom -from openpilot.common.conversions import Conversions as CV from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert @@ -16,17 +14,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'].slc.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.) - - class EventsSP(EventsBase): def __init__(self): super().__init__() @@ -145,34 +132,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { EventNameSP.pedalPressedAlertOnly: { ET.WARNING: NoEntryAlert("Pedal Pressed") - }, - - EventNameSP.speedLimitPreActive: { - ET.WARNING: Alert( - "", - "", - AlertStatus.normal, AlertSize.none, - Priority.MID, VisualAlert.none, AudibleAlert.none, .45), # TODO-SP: AudibleAlert.promptSingleLow - }, - - EventNameSP.speedLimitActive: { - ET.WARNING: Alert( - "Set speed changed to match posted speed limit", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 3.), - }, - - EventNameSP.speedLimitConfirmed: { - ET.WARNING: Alert( - "", - "", - AlertStatus.normal, AlertSize.none, - Priority.MID, VisualAlert.none, AudibleAlert.none, .45), # TODO-SP: AudibleAlert.promptSingleHigh - }, - - EventNameSP.speedLimitValueChange: { - ET.WARNING: speed_limit_adjust_alert, - }, + } } diff --git a/system/manager/manager.py b/system/manager/manager.py index 7c53483714..cdad098d2d 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -63,15 +63,6 @@ def manager_init() -> None: ("ModelManager_ModelsCache", ""), ("NeuralNetworkLateralControl", "0"), ("QuietMode", "0"), - - ("SpeedLimitControl", "0"), - ("SpeedLimitControlPolicy", "3"), - ("SpeedLimitEngageType", "0"), - ("SpeedLimitOffsetType", "0"), - ("SpeedLimitValueOffset", "0"), - ("SpeedLimitWarningType", "0"), - ("SpeedLimitWarningOffsetType", "0"), - ("SpeedLimitWarningValueOffset", "0"), ] if params.get_bool("RecordFrontLock"): diff --git a/system/manager/process_config.py b/system/manager/process_config.py index ab0ffe93a6..2e3f754a76 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -6,8 +6,9 @@ from cereal import car, custom from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess +from openpilot.system.hardware.hw import Paths -from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH, COMMON_DIR +from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH from sunnypilot.models.helpers import get_active_model_runner from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, use_sunnylink_uploader @@ -161,7 +162,7 @@ procs += [ PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)), # mapd - NativeProcess("mapd", COMMON_DIR, [MAPD_PATH], always_run), + NativeProcess("mapd", Paths.mapd_root(), [MAPD_PATH], always_run), PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), ]