Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

selfdrive/controls/controlsd.py

@@ -124,7 +124,6 @@ class Controls(ControlsExt, ModelStateBase):
 
       self.LaC.extension.update_model_v2(self.sm['modelV2'])
 
-      self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay)
       self.LaC.extension.update_lateral_lag(self.lat_delay)
 
     long_plan = self.sm['longitudinalPlan']
@@ -276,6 +275,9 @@ class Controls(ControlsExt, ModelStateBase):
     while not evt.is_set():
       self.get_params_sp()
 
+      if self.CP.lateralTuning.which() == 'torque':
+        self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay)
+
       time.sleep(0.1)
 
   def run(self):

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc

@@ -7,6 +7,8 @@
 
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h"
 
+#include "selfdrive/ui/sunnypilot/qt/util.h"
+
 SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) {
   subPanelFrame = new QFrame();
   QVBoxLayout *subPanelLayout = new QVBoxLayout(subPanelFrame);
@@ -109,7 +111,7 @@ void SpeedLimitSettings::refresh() {
   QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));
 
   bool has_longitudinal_control;
-  bool intelligent_cruise_button_management_available;
+  bool has_icbm;
   auto cp_bytes = params.get("CarParamsPersistent");
   auto cp_sp_bytes = params.get("CarParamsSPPersistent");
   if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
@@ -121,16 +123,16 @@ void SpeedLimitSettings::refresh() {
     cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
 
     has_longitudinal_control = hasLongitudinalControl(CP);
-    intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
+    has_icbm = hasIntelligentCruiseButtonManagement(CP_SP);
 
-    if (!has_longitudinal_control && CP_SP.getPcmCruiseSpeed()) {
+    if (!has_longitudinal_control && !has_icbm) {
       if (speed_limit_mode_param == SpeedLimitMode::ASSIST) {
         params.put("SpeedLimitMode", std::to_string(static_cast<int>(SpeedLimitMode::WARNING)));
       }
     }
   } else {
     has_longitudinal_control = false;
-    intelligent_cruise_button_management_available = false;
+    has_icbm = false;
   }
 
   speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param));
@@ -150,13 +152,14 @@ void SpeedLimitSettings::refresh() {
     speed_limit_offset->showDescription();
   }
 
-  if (has_longitudinal_control || intelligent_cruise_button_management_available) {
+  if (has_longitudinal_control || has_icbm) {
     speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues()));
   } else {
     speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(
       {SpeedLimitMode::OFF, SpeedLimitMode::INFORMATION, SpeedLimitMode::WARNING}));
   }
 
+  speed_limit_mode_settings->refresh();
   speed_limit_mode_settings->showDescription();
   speed_limit_offset->showDescription();
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h

@@ -35,6 +35,7 @@ private:
   SpeedLimitPolicy *speedLimitPolicyScreen;
   ButtonParamControlSP *speed_limit_offset_settings;
   OptionControlSP *speed_limit_offset;
+  bool icbm_available = false;
 
   static QString offsetDescription(SpeedLimitOffsetType type = SpeedLimitOffsetType::NONE) {
     QString none_str = tr("⦿ None: No Offset");

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc

@@ -7,6 +7,8 @@
 
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h"
 
+#include "selfdrive/ui/sunnypilot/qt/util.h"
+
 LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
   setStyleSheet(R"(
     #back_btn {
@@ -40,7 +42,9 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
     "",
     this
   );
-  intelligentCruiseButtonManagement->setConfirmation(true, false);
+  QObject::connect(intelligentCruiseButtonManagement, &ParamControlSP::toggleFlipped, this, [=](bool) {
+    refresh(offroad);
+  });
   list->addItem(intelligentCruiseButtonManagement);
 
   dynamicExperimentalControl = new ParamControlSP(
@@ -112,22 +116,41 @@ void LongitudinalPanel::refresh(bool _offroad) {
 
     has_longitudinal_control = hasLongitudinalControl(CP);
     is_pcm_cruise = CP.getPcmCruise();
-    intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
+    has_icbm = hasIntelligentCruiseButtonManagement(CP_SP);
 
-    if (!intelligent_cruise_button_management_available || has_longitudinal_control) {
+    if (CP_SP.getIntelligentCruiseButtonManagementAvailable() && !has_longitudinal_control) {
+      intelligentCruiseButtonManagement->setEnabled(offroad);
+    } else {
       params.remove("IntelligentCruiseButtonManagement");
+      intelligentCruiseButtonManagement->setEnabled(false);
     }
 
-    if (!has_longitudinal_control && CP_SP.getPcmCruiseSpeed()) {
+    if (has_longitudinal_control || has_icbm) {
+      // enable Custom ACC Increments when long is available and is not PCM cruise
+      customAccIncrement->setEnabled(((has_longitudinal_control && !is_pcm_cruise) || has_icbm) && offroad);
+      dynamicExperimentalControl->setEnabled(has_longitudinal_control);
+      SmartCruiseControlVision->setEnabled(true);
+      SmartCruiseControlMap->setEnabled(true);
+    } else {
       params.remove("CustomAccIncrementsEnabled");
       params.remove("DynamicExperimentalControl");
       params.remove("SmartCruiseControlVision");
       params.remove("SmartCruiseControlMap");
+      customAccIncrement->setEnabled(false);
+      dynamicExperimentalControl->setEnabled(false);
+      SmartCruiseControlVision->setEnabled(false);
+      SmartCruiseControlMap->setEnabled(false);
     }
+
+    intelligentCruiseButtonManagement->refresh();
+    customAccIncrement->refresh();
+    dynamicExperimentalControl->refresh();
+    SmartCruiseControlVision->refresh();
+    SmartCruiseControlMap->refresh();
   } else {
     has_longitudinal_control = false;
     is_pcm_cruise = false;
-    intelligent_cruise_button_management_available = false;
+    has_icbm = false;
   }
 
   QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease.");
@@ -139,8 +162,8 @@ void LongitudinalPanel::refresh(bool _offroad) {
     customAccIncrement->setDescription(onroadOnlyDescription);
     customAccIncrement->showDescription();
   } else {
-    if (has_longitudinal_control || intelligent_cruise_button_management_available) {
-      if (is_pcm_cruise) {
+    if (has_longitudinal_control || has_icbm) {
+      if (has_longitudinal_control && is_pcm_cruise) {
         customAccIncrement->setDescription(accPcmCruiseDisabledDescription);
         customAccIncrement->showDescription();
       } else {
@@ -150,21 +173,8 @@ void LongitudinalPanel::refresh(bool _offroad) {
       customAccIncrement->toggleFlipped(false);
       customAccIncrement->setDescription(accNoLongDescription);
       customAccIncrement->showDescription();
-      intelligentCruiseButtonManagement->toggleFlipped(false);
     }
   }
 
-  bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control;
-  intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad);
-
-  // enable toggle when long is available and is not PCM cruise
-  bool cai_allowed = (has_longitudinal_control && !is_pcm_cruise) || icbm_allowed;
-  customAccIncrement->setEnabled(cai_allowed && !offroad);
-  customAccIncrement->refresh();
-
-  dynamicExperimentalControl->setEnabled(has_longitudinal_control);
-  SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed);
-  SmartCruiseControlMap->setEnabled(has_longitudinal_control || icbm_allowed);
-
   offroad = _offroad;
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h

@@ -25,7 +25,7 @@ private:
   Params params;
   bool has_longitudinal_control = false;
   bool is_pcm_cruise = false;
-  bool intelligent_cruise_button_management_available = false;;
+  bool has_icbm = false;
   bool offroad = false;
 
   QStackedLayout *main_layout = nullptr;

selfdrive/ui/sunnypilot/qt/util.cc

@@ -122,3 +122,7 @@ std::optional<cereal::Event::Reader> loadCerealEvent(Params& params, const std::
     return std::nullopt;
   }
 }
+
+bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp) {
+  return car_params_sp.getIntelligentCruiseButtonManagementAvailable() && Params().getBool("IntelligentCruiseButtonManagement");
+}

selfdrive/ui/sunnypilot/qt/util.h

@@ -23,3 +23,4 @@ std::optional<QString> getParamIgnoringDefault(const std::string &param_name, co
 QMap<QString, QVariantMap> loadPlatformList();
 QStringList searchFromList(const QString &query, const QStringList &list);
 std::optional<cereal::Event::Reader> loadCerealEvent(Params& params, const std::string& _param);
+bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp);

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py

@@ -27,7 +27,11 @@ ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
 ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
 
 DISABLED_GUARD_PERIOD = 0.5  # secs.
-PRE_ACTIVE_GUARD_PERIOD = 15  # secs. Time to wait after activation before considering temp deactivation signal.
+# secs. Time to wait after activation before considering temp deactivation signal.
+PRE_ACTIVE_GUARD_PERIOD = {
+  True: 15,
+  False: 5,
+}
 SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1  # secs. Time to wait after speed limit change before switching to preActive.
 
 LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
@@ -109,6 +113,16 @@ class SpeedLimitAssist:
   def target_set_speed_confirmed(self) -> bool:
     return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
 
+  @property
+  def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
+    return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
+
+  def update_active_event(self, events_sp: EventsSP) -> None:
+    if self.v_cruise_cluster_below_confirm_speed_threshold:
+      events_sp.add(EventNameSP.speedLimitChanged)
+    else:
+      events_sp.add(EventNameSP.speedLimitActive)
+
   def get_v_target_from_control(self) -> float:
     if self._has_speed_limit:
       if self.pcm_op_long and self.is_enabled:
@@ -175,7 +189,7 @@ class SpeedLimitAssist:
   @property
   def apply_confirm_speed_threshold(self) -> bool:
     # below CST: always require user confirmation
-    if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]:
+    if self.v_cruise_cluster_below_confirm_speed_threshold:
       return True
 
     # at/above CST:
@@ -231,7 +245,7 @@ class SpeedLimitAssist:
             self.state = SpeedLimitAssistState.inactive
           elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
           elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
             self.state = SpeedLimitAssistState.adapting
 
@@ -241,7 +255,7 @@ class SpeedLimitAssist:
             self.state = SpeedLimitAssistState.inactive
           elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
           elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
             self.state = SpeedLimitAssistState.active
 
@@ -251,7 +265,7 @@ class SpeedLimitAssist:
             self._update_confirmed_state()
           elif self.speed_limit_changed:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
 
         # PRE_ACTIVE
         elif self.state == SpeedLimitAssistState.preActive:
@@ -277,7 +291,7 @@ class SpeedLimitAssist:
             self._update_confirmed_state()
           elif self._has_speed_limit:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
           else:
             self.state = SpeedLimitAssistState.pending
 
@@ -303,7 +317,7 @@ class SpeedLimitAssist:
 
           elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
 
         # PRE_ACTIVE
         elif self.state == SpeedLimitAssistState.preActive:
@@ -317,7 +331,7 @@ class SpeedLimitAssist:
         elif self.state == SpeedLimitAssistState.inactive:
           if self.speed_limit_changed:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
           elif self._update_non_pcm_long_confirmed_state():
             self.state = SpeedLimitAssistState.active
 
@@ -333,7 +347,7 @@ class SpeedLimitAssist:
             self.state = SpeedLimitAssistState.active
           elif self._has_speed_limit:
             self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
           else:
             self.state = SpeedLimitAssistState.inactive
 
@@ -351,15 +365,15 @@ class SpeedLimitAssist:
 
     if self.is_active:
       if self._state_prev not in ACTIVE_STATES:
-        events_sp.add(EventNameSP.speedLimitActive)
+        self.update_active_event(events_sp)
 
       # only notify if we acquire a valid speed limit
       # do not check has_speed_limit here
       elif self._speed_limit != self.speed_limit_prev:
         if self.speed_limit_prev <= 0:
-          events_sp.add(EventNameSP.speedLimitActive)
+          self.update_active_event(events_sp)
         elif self.speed_limit_prev > 0 and self._speed_limit > 0:
-          events_sp.add(EventNameSP.speedLimitChanged)
+          self.update_active_event(events_sp)
 
   def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
              speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -39,7 +39,7 @@ class TestSpeedLimitAssist:
     self.events_sp = EventsSP()
     CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2)
     self.sla = SpeedLimitAssist(CI.CP)
-    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+    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
 
@@ -114,7 +114,7 @@ class TestSpeedLimitAssist:
     self.sla.state = SpeedLimitAssistState.preActive
     self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
 
-    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
+    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
       self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.inactive
 

sunnypilot/selfdrive/selfdrived/events.py

@@ -4,7 +4,6 @@ from openpilot.common.constants import CV
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -34,6 +33,9 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag
   speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
   speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
   speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
+  alert_1_str = ""
+  alert_2_str = ""
+  alert_size = AlertSize.none
 
   if CP.openpilotLongitudinalControl and CP.pcmCruise:
     # PCM long
@@ -41,24 +43,15 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag
     pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
     pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
     speed_unit = "km/h" if metric else "mph"
+
+    alert_1_str = "Speed Limit Assist: Activation Required"
     alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
-  else:
-    # Non PCM long
-    v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS
-
-    req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric)
-    arrow_str = ""
-    if req_plus:
-      arrow_str = "RES/+"
-    elif req_minus:
-      arrow_str = "SET/-"
-
-    alert_2_str = f"Operate the {arrow_str} cruise control button to activate"
+    alert_size = AlertSize.mid
 
   return Alert(
-    "Speed Limit Assist: Activation Required",
+    alert_1_str,
     alert_2_str,
-    AlertStatus.normal, AlertSize.mid,
+    AlertStatus.normal, alert_size,
     Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)