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.

selfdrive/ui/qt/offroad/settings.cc

@@ -172,7 +172,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

@@ -178,7 +178,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;
@@ -548,7 +548,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;
@@ -655,7 +655,7 @@ void AnnotatedCameraWidget::paintGL() {
       // for replay of old routes, never go to widecam
       wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
     }
-    CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER : 
+    CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER :
                                 wide_cam_requested && cameraView != 1 ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
 
     s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD;
@@ -742,9 +742,13 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
 void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   alwaysOnLateral = scene.always_on_lateral_active;
   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);
   }
 
@@ -775,8 +779,26 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.setOpacity(1.0);
   p.drawRoundedRect(statusBarRect, 30, 30);
 
+  QMap<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 navigation" + (QString(" instructions input"))},
+    {6, "Experimental Mode activated due to" + (QString(" no speed limit set"))},
+    {7, "Experimental Mode activated due to" + (" speed being less than " + QString::number(conditionalSpeedLead) + (is_metric ? " kph" : " mph"))},
+    {8, "Experimental Mode activated due to" + (" speed being less than " + QString::number(conditionalSpeed) + (is_metric ? " kph" : " mph"))},
+    {9, "Experimental Mode activated for slower lead"},
+    {10, "Experimental Mode activated for turn" + (QString(" / lane change"))},
+    {11, "Experimental Mode activated for curve"},
+    {12, "Experimental Mode activated for stop" + (QString(" sign / stop light"))}
+  };
+
   if (alwaysOnLateral) {
     newStatus = QString("Always On Lateral active") + (". Press the \"Cruise Control\" button to disable");
+  } else if (conditionalExperimental) {
+    newStatus = conditionalStatusMap.contains(conditionalStatus) && status != STATUS_DISENGAGED ? conditionalStatusMap[conditionalStatus] : conditionalStatusMap[0];
   }
 
   // Check if status has changed

selfdrive/ui/qt/onroad.h

@@ -118,8 +118,12 @@ private:
   QHBoxLayout *bottom_layout;
 
   bool alwaysOnLateral;
+  bool conditionalExperimental;
   bool experimentalMode;
   int cameraView;
+  int conditionalSpeed;
+  int conditionalSpeedLead;
+  int conditionalStatus;
 
 protected:
   void paintGL() override;

selfdrive/ui/ui.cc

@@ -250,6 +250,10 @@ void ui_update_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");
 }
 
 void UIState::updateStatus() {
@@ -318,6 +322,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

@@ -171,9 +171,13 @@ typedef struct UIScene {
   // FrogPilot variables
   bool always_on_lateral;
   bool always_on_lateral_active;
+  bool conditional_experimental;
   bool enabled;
   bool experimental_mode;
   int camera_view;
+  int conditional_speed;
+  int conditional_speed_lead;
+  int conditional_status;
 
 } UIScene;