Conditional Experimental Mode

Added toggles for "Conditional Experimental Mode".

Conditions based on road curvature, turn signals, speed, lead speed, navigation instructions, and stop signs/stop lights are all individually toggleable.

Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>

selfdrive/ui/qt/offroad/settings.cc

@@ -174,7 +174,7 @@ void TogglesPanel::updateToggles() {
     op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release);
     if (hasLongitudinalControl(CP)) {
       // normal description and toggle
-      experimental_mode_toggle->setEnabled(true);
+      experimental_mode_toggle->setEnabled(!params.getBool("ConditionalExperimental"));
       experimental_mode_toggle->setDescription(e2e_description);
       long_personality_setting->setEnabled(true);
     } else {

selfdrive/ui/qt/onroad.cc

@@ -175,7 +175,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
   int margin = 40;
   int radius = 30;
-  int offset = scene.always_on_lateral ? 25 : 0;
+  int offset = scene.always_on_lateral || scene.conditional_experimental ? 25 : 0;
   if (alert.size == cereal::ControlsState::AlertSize::FULL) {
     margin = 0;
     radius = 0;
@@ -563,7 +563,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
 
   // base icon
   int offset = UI_BORDER_SIZE + btn_size / 2;
-  offset += alwaysOnLateral ? 25 : 0;
+  offset += alwaysOnLateral || conditionalExperimental ? 25 : 0;
   int x = rightHandDM ? width() - offset : offset;
   int y = height() - offset;
   float opacity = dmActive ? 0.65 : 0.2;
@@ -762,9 +762,14 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
 
   cameraView = scene.camera_view;
 
+  conditionalExperimental = scene.conditional_experimental;
+  conditionalSpeed = scene.conditional_speed;
+  conditionalSpeedLead = scene.conditional_speed_lead;
+  conditionalStatus = scene.conditional_status;
+
   experimentalMode = scene.experimental_mode;
 
-  if (alwaysOnLateral) {
+  if (alwaysOnLateral || conditionalExperimental) {
     drawStatusBar(p);
   }
 
@@ -796,9 +801,28 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.setOpacity(1.0);
   p.drawRoundedRect(statusBarRect, 30, 30);
 
+  std::map<int, QString> conditionalStatusMap = {
+    {0, "Conditional Experimental Mode ready"},
+    {1, "Conditional Experimental overridden"},
+    {2, "Experimental Mode manually activated"},
+    {3, "Conditional Experimental overridden"},
+    {4, "Experimental Mode manually activated"},
+    {5, "Experimental Mode activated for" + (mapOpen ? " intersection" : QString(" upcoming intersection"))},
+    {6, "Experimental Mode activated for" + (mapOpen ? " turn" : QString(" upcoming turn"))},
+    {7, "Experimental Mode activated due to" + (mapOpen ? "SLC" : QString(" no speed limit set"))},
+    {8, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeedLead) + (is_metric ? " kph" : " mph"))},
+    {9, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeed) + (is_metric ? " kph" : " mph"))},
+    {10, "Experimental Mode activated for slower lead"},
+    {11, "Experimental Mode activated for turn" + (mapOpen ? "" : QString(" / lane change"))},
+    {12, "Experimental Mode activated for curve"},
+    {13, "Experimental Mode activated for stop" + (mapOpen ? "" : QString(" sign / stop light"))},
+  };
+
   // Update status text
   if (alwaysOnLateralActive) {
     newStatus = QString("Always On Lateral active. Press the \"Cruise Control\" button to disable");
+  } else if (conditionalExperimental) {
+    newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0];
   }
 
   // Check if status has changed

selfdrive/ui/qt/onroad.h

@@ -122,9 +122,13 @@ private:
   bool alwaysOnLateralActive;
   bool blindSpotLeft;
   bool blindSpotRight;
+  bool conditionalExperimental;
   bool experimentalMode;
 
   int cameraView;
+  int conditionalSpeed;
+  int conditionalSpeedLead;
+  int conditionalStatus;
 
 protected:
   void paintGL() override;

selfdrive/ui/ui.cc

@@ -262,6 +262,10 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.always_on_lateral = params.getBool("AlwaysOnLateral");
   scene.camera_view = params.getInt("CameraView");
 
+  scene.conditional_experimental = params.getBool("ConditionalExperimental");
+  scene.conditional_speed = params.getInt("CESpeed");
+  scene.conditional_speed_lead = params.getInt("CESpeedLead");
+
   bool custom_onroad_ui = params.getBool("CustomUI");
   scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath");
   scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath");
@@ -337,6 +341,9 @@ void UIState::update() {
   }
 
   // FrogPilot live variables that need to be constantly checked
+  if (scene.conditional_experimental) {
+    scene.conditional_status = paramsMemory.getInt("CEStatus");
+  }
 }
 
 void UIState::setPrimeType(PrimeType type) {

selfdrive/ui/ui.h

@@ -175,6 +175,7 @@ typedef struct UIScene {
   bool blind_spot_left;
   bool blind_spot_path;
   bool blind_spot_right;
+  bool conditional_experimental;
   bool enabled;
   bool experimental_mode;
 
@@ -182,6 +183,9 @@ typedef struct UIScene {
   float lane_width_right;
 
   int camera_view;
+  int conditional_speed;
+  int conditional_speed_lead;
+  int conditional_status;
 
   QPolygonF track_adjacent_vertices[6];