diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py index af14a87f0c..696d9c6cc6 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py @@ -6,3 +6,7 @@ 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 Auto mode constants +REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params +CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index 37ca52b647..c1bb6c5234 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -13,6 +13,7 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode from openpilot.selfdrive.modeld.constants import ModelConstants @@ -31,11 +32,6 @@ LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers 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. -# Speed Limit Assist Auto mode constants -REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params -CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial -FALLBACK_CRUISE_SPEED = 255.0 # m/s fallback when no speed limit available - class SpeedLimitAssist: _speed_limit: float 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 index a764a2327a..ff468f02c4 100644 --- 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 @@ -14,8 +14,9 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import REQUIRED_INITIAL_MAX_SET_SPEED from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \ - REQUIRED_INITIAL_MAX_SET_SPEED, PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES + PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 47ea59a86c..6fc93c922e 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -3,6 +3,7 @@ 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 REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE AlertSize = log.SelfdriveState.AlertSize @@ -27,6 +28,18 @@ def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. 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: + max_initial_set_speed = round(REQUIRED_INITIAL_MAX_SET_SPEED * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) + if metric: + max_initial_set_speed += round(CRUISE_SPEED_TOLERANCE * CV.MS_TO_KPH) + speed_unit = "km/h" if metric else "mph" + return Alert( + "Speed Limit Assist: Activation Required", + f"Manually change set speed to {max_initial_set_speed} {speed_unit} to activate", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.) + + class EventsSP(EventsBase): def __init__(self): super().__init__() @@ -180,11 +193,7 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { }, EventNameSP.speedLimitPreActive: { - ET.WARNING: Alert( - "Speed Limit Assist: Activation Required", - "Manually change set speed to 80 MPH to activate", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + ET.WARNING: speed_limit_pre_active_alert, }, EventNameSP.speedLimitPending: {