From 7097e69aa320e269b9c59d70756da23588a8aeef Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 22 Oct 2025 11:17:02 -0400 Subject: [PATCH] Speed Limit Assist: generalize availability helper (#1416) * init infra to disable sla in certain conditions * a bit more * in another PR * in another PR * since when? * start here --- .../controls/lib/longitudinal_planner.py | 4 ++-- selfdrive/controls/plannerd.py | 8 +++++-- .../test/longitudinal_maneuvers/plant.py | 4 +++- .../speed_limit/speed_limit_settings.cc | 24 ++++++++++--------- sunnypilot/selfdrive/car/interfaces.py | 5 ++-- .../controls/lib/longitudinal_planner.py | 4 ++-- .../controls/lib/speed_limit/helpers.py | 17 +++++++++++++ .../lib/speed_limit/speed_limit_assist.py | 9 ++++--- .../tests/test_speed_limit_assist.py | 2 +- 9 files changed, 52 insertions(+), 25 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 53eca3835..0501f669f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -51,12 +51,12 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner(LongitudinalPlannerSP): - def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): + def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) # TODO remove mpc modes when TR released self.mpc.mode = 'acc' - LongitudinalPlannerSP.__init__(self, self.CP, self.mpc) + LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 4565a9c04..f7d3370f9 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process @@ -17,10 +17,14 @@ def main(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("plannerd got CarParams: %s", CP.brand) + cloudlog.info("plannerd is waiting for CarParamsSP") + CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP) + cloudlog.info("plannerd got CarParamsSP") + gps_location_service = get_gps_location_service(params) ldw = LaneDepartureWarning() - longitudinal_planner = LongitudinalPlanner(CP) + longitudinal_planner = LongitudinalPlanner(CP, CP_SP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'liveMapDataSP', 'carStateSP', gps_location_service], diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index e46647243..e82ef2f65 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -51,7 +51,9 @@ class Plant: from opendbc.car.honda.values import CAR from opendbc.car.honda.interface import CarInterface - self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=self.speed) + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC) + self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed) @property def current_time(self): diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc index 1696c7f63..50d9ccbb4 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -110,8 +110,7 @@ void SpeedLimitSettings::refresh() { SpeedLimitOffsetType offset_type_param = static_cast(std::atoi(params.get("SpeedLimitOffsetType").c_str())); QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); - bool has_longitudinal_control; - bool has_icbm; + bool sla_available; auto cp_bytes = params.get("CarParamsPersistent"); auto cp_sp_bytes = params.get("CarParamsSPPersistent"); if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { @@ -122,17 +121,20 @@ void SpeedLimitSettings::refresh() { cereal::CarParams::Reader CP = cmsg.getRoot(); cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); - has_longitudinal_control = hasLongitudinalControl(CP); - has_icbm = hasIntelligentCruiseButtonManagement(CP_SP); + bool has_longitudinal_control = hasLongitudinalControl(CP); + bool has_icbm = hasIntelligentCruiseButtonManagement(CP_SP); - if (!has_longitudinal_control && !has_icbm) { - if (speed_limit_mode_param == SpeedLimitMode::ASSIST) { - params.put("SpeedLimitMode", std::to_string(static_cast(SpeedLimitMode::WARNING))); - } + /* + * Speed Limit Assist is available when: + * - has_longitudinal_control or has_icbm + */ + sla_available = has_longitudinal_control || has_icbm; + + if (!sla_available && speed_limit_mode_param == SpeedLimitMode::ASSIST) { + params.put("SpeedLimitMode", std::to_string(static_cast(SpeedLimitMode::WARNING))); } } else { - has_longitudinal_control = false; - has_icbm = false; + sla_available = false; } speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param)); @@ -152,7 +154,7 @@ void SpeedLimitSettings::refresh() { speed_limit_offset->showDescription(); } - if (has_longitudinal_control || has_icbm) { + if (sla_available) { speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues())); } else { speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues( diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 6a868ab85..7d63d3c5b 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -11,7 +11,7 @@ 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.common import Mode as SpeedLimitMode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability import openpilot.system.sentry as sentry @@ -87,8 +87,7 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS params.remove("SmartCruiseControlVision") params.remove("SmartCruiseControlMap") - if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist: - params.put("SpeedLimitMode", int(SpeedLimitMode.warning)) + 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/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 4c2443f25..68b50c3e1 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -22,13 +22,13 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: - def __init__(self, CP: structs.CarParams, mpc): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc): self.events_sp = EventsSP() self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() - self.sla = SpeedLimitAssist(CP) + self.sla = SpeedLimitAssist(CP, CP_SP) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = LongitudinalPlanSource.cruise self.e2e_alerts_helper = E2EAlertsHelper() diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py index 40e8f653f..c209e98dc 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py @@ -5,7 +5,10 @@ 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]: @@ -17,3 +20,17 @@ def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_ 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) -> None: + if params is None: + params = Params() + + allowed = True + + 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)) 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 b94845205..fcb5a98a2 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -15,7 +15,7 @@ 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 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 +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability from openpilot.selfdrive.modeld.constants import ModelConstants ButtonType = car.CarState.ButtonEvent.Type @@ -38,7 +38,7 @@ LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers 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 +V_CRUISE_UNSET = 255. CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise) CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise) @@ -52,13 +52,15 @@ class SpeedLimitAssist: a_ego: float v_offset: float - def __init__(self, CP): + 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 @@ -140,6 +142,7 @@ class SpeedLimitAssist: 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: 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 168c53016..8a3376a05 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 @@ -38,7 +38,7 @@ class TestSpeedLimitAssist: self.reset_custom_params() self.events_sp = EventsSP() CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2) - self.sla = SpeedLimitAssist(CI.CP) + 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