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

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

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],

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):

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<SpeedLimitOffsetType>(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::CarParams>();
     cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
 
-    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<int>(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<int>(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(

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:

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()

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))

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:

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