Controls - Experimental Mode Activation

Toggle Experimental Mode with either buttons on the steering wheel or the screen. Overrides 'Conditional Experimental Mode'.

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -22,12 +22,17 @@ class ConditionalExperimentalMode:
     self.stop_light_detected = False
 
   def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles):
-    if not carState.standstill:
+    if frogpilot_toggles.experimental_mode_via_press:
+      self.status_value = self.params_memory.get_int("CEStatus")
+    else:
+      self.status_value = 0
+
+    if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill:
       self.update_conditions(self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles)
       self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles)
       self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0)
     else:
-      self.experimental_mode = carState.standstill and self.experimental_mode
+      self.experimental_mode = self.status_value in {2, 4, 6} or carState.standstill and self.experimental_mode
 
   def check_conditions(self, carState, frogpilotNavigation, modelData, tracking_lead, v_ego, v_lead, frogpilot_toggles):
     below_speed = frogpilot_toggles.conditional_limit > v_ego >= 1 and not tracking_lead

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -551,6 +551,12 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
 
   std::map<int, QString> conditionalStatusMap = {
     {0, tr("Conditional Experimental Mode ready")},
+    {1, tr("Conditional Experimental overridden")},
+    {2, tr("Experimental Mode manually activated")},
+    {3, tr("Conditional Experimental overridden")},
+    {4, tr("Experimental Mode manually activated")},
+    {5, tr("Conditional Experimental overridden")},
+    {6, tr("Experimental Mode manually activated")},
     {7, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeedLead) + (is_metric ? tr("kph") : tr("mph")))},
     {8, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeed) + (is_metric ? tr("kph") : tr("mph")))},
     {9, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))},
@@ -568,6 +574,21 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
     newStatus = conditionalStatusMap.at(conditionalStatus);
   }
 
+  static const std::map<int, QString> suffixMap = {
+    {1, tr(". Long press the \"distance\" button to revert")},
+    {2, tr(". Long press the \"distance\" button to revert")},
+    {3, tr(". Click the \"LKAS\" button to revert")},
+    {4, tr(". Click the \"LKAS\" button to revert")},
+    {5, tr(". Double tap the screen to revert")},
+    {6, tr(". Double tap the screen to revert")},
+  };
+
+  if (!alwaysOnLateralActive && !mapOpen && !newStatus.isEmpty()) {
+    if (suffixMap.find(conditionalStatus) != suffixMap.end()) {
+      newStatus += suffixMap.at(conditionalStatus);
+    }
+  }
+
   if (newStatus != lastShownStatus) {
     lastShownStatus = newStatus;
     displayStatusText = true;

selfdrive/ui/qt/onroad/buttons.cc

@@ -28,7 +28,12 @@ void ExperimentalButton::changeMode() {
   const auto cp = (*uiState()->sm)["carParams"].getCarParams();
   bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
   if (can_change) {
-    params.putBool("ExperimentalMode", !experimental_mode);
+    if (conditionalExperimental) {
+      int override_value = (conditionalStatus >= 1 && conditionalStatus <= 6) ? 0 : conditionalStatus >= 7 ? 5 : 6;
+      paramsMemory.putIntNonBlocking("CEStatus", override_value);
+    } else {
+      params.putBool("ExperimentalMode", !experimental_mode);
+    }
   }
 }
 
@@ -43,6 +48,9 @@ void ExperimentalButton::updateState(const UIState &s) {
 
   // FrogPilot variables
   const UIScene &scene = s.scene;
+
+  conditionalExperimental = scene.conditional_experimental;
+  conditionalStatus = scene.conditional_status;
 }
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {

selfdrive/ui/qt/onroad/buttons.h

@@ -25,6 +25,10 @@ private:
   bool engageable;
 
   // FrogPilot variables
+  bool conditionalExperimental;
+
+  int conditionalStatus;
+
   Params paramsMemory{"/dev/shm/params"};
 };
 

selfdrive/ui/ui.h

@@ -57,6 +57,7 @@ typedef enum UIStatus {
 
   // FrogPilot statuses
   STATUS_ALWAYS_ON_LATERAL_ACTIVE,
+  STATUS_CONDITIONAL_OVERRIDDEN,
   STATUS_EXPERIMENTAL_MODE_ACTIVE,
   STATUS_NAVIGATION_ACTIVE,
 } UIStatus;
@@ -79,6 +80,7 @@ const QColor bg_colors [] = {
 
   // FrogPilot colors
   [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
+  [STATUS_CONDITIONAL_OVERRIDDEN] = QColor(0xff, 0xff, 0x00, 0xf1),
   [STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1),
   [STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1),
 };