Controls - Conditional Experimental Mode

Automatically switches to 'Experimental Mode' under predefined conditions.

selfdrive/controls/controlsd.py

@@ -862,7 +862,8 @@ class Controls:
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
-      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode:
+        self.experimental_mode = self.params.get_bool("ExperimentalMode")
       self.personality = self.read_personality_param()
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@@ -919,6 +920,9 @@ class Controls:
         self.params_tracking.put_int_nonblocking("FrogPilotDrives", self.total_drives)
         self.drive_added = True
 
+    if self.frogpilot_toggles.conditional_experimental_mode:
+      self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimentalActive
+
     FPCC = custom.FrogPilotCarControl.new_message()
     FPCC.alwaysOnLateral = self.always_on_lateral_active
 

selfdrive/controls/lib/drive_helpers.py

@@ -134,7 +134,7 @@ class VCruiseHelper:
     if self.CP.pcmCruise:
       return
 
-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
+    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not frogpilot_toggles.conditional_experimental_mode else V_CRUISE_INITIAL
 
     # 250kph or above probably means we never had a set speed
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
+from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, TRAJECTORY_SIZE
 
@@ -21,6 +22,8 @@ class FrogPilotPlanner:
   def __init__(self):
     self.params_memory = Params("/dev/shm/params")
 
+    self.cem = ConditionalExperimentalMode()
+
     self.tracking_lead = False
 
     self.acceleration_jerk = 0
@@ -40,6 +43,9 @@ class FrogPilotPlanner:
     lead_distance = self.lead_one.dRel
     stopping_distance = STOP_DISTANCE
 
+    if frogpilot_toggles.conditional_experimental_mode and controlsState.enabled:
+      self.cem.update(carState, frogpilotNavigation, self.lead_one, modelData, self.model_length, self.road_curvature, self.slower_lead, self.tracking_lead, v_ego, v_lead, frogpilot_toggles)
+
     if v_ego >= frogpilot_toggles.minimum_lane_change_speed:
       self.lane_width_left = float(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]))
       self.lane_width_right = float(calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]))
@@ -106,6 +112,8 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk)
     frogpilotPlan.tFollow = float(self.t_follow)
 
+    frogpilotPlan.conditionalExperimentalActive = bool(self.cem.experimental_mode)
+
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
 

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -0,0 +1,27 @@
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY, TRAJECTORY_SIZE
+
+class ConditionalExperimentalMode:
+  def __init__(self):
+    self.params_memory = Params("/dev/shm/params")
+
+    self.experimental_mode = False
+
+  def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles):
+    if not carState.standstill:
+      self.update_conditions(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles)
+      self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, 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
+
+  def check_conditions(self, carState, frogpilotNavigation, modelData, tracking_lead, v_ego, v_lead, frogpilot_toggles):
+    if (tracking_lead and v_ego <= frogpilot_toggles.conditional_limit_lead) or (not tracking_lead and v_ego <= frogpilot_toggles.conditional_limit):
+      self.status_value = 7 if tracking_lead else 8
+      return True
+
+    return False
+
+  def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles):

selfdrive/ui/qt/offroad/settings.cc

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

selfdrive/ui/qt/onroad/alerts.cc

@@ -16,6 +16,7 @@ void OnroadAlerts::updateState(const UIState &s) {
   const UIScene &scene = s.scene;
 
   showAOLStatusBar = scene.show_aol_status_bar;
+  showCEMStatusBar = scene.show_cem_status_bar;
 }
 
 void OnroadAlerts::clear() {
@@ -72,7 +73,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
   int margin = 40;
   int radius = 30;
-  int offset = showAOLStatusBar ? 25 : 0;
+  int offset = showAOLStatusBar || showCEMStatusBar ? 25 : 0;
   if (alert.size == cereal::ControlsState::AlertSize::FULL) {
     margin = 0;
     radius = 0;

selfdrive/ui/qt/onroad/alerts.h

@@ -42,4 +42,5 @@ protected:
 
   // FrogPilot variables
   bool showAOLStatusBar;
+  bool showCEMStatusBar;
 };

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -283,7 +283,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
   // base icon
   int offset = UI_BORDER_SIZE + btn_size / 2;
   int x = rightHandDM ? width() - offset : offset;
-  offset += showAlwaysOnLateralStatusBar ? 25 : 0;
+  offset += showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar ? 25 : 0;
   int y = height() - offset;
   float opacity = dmActive ? 0.65 : 0.2;
   drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
@@ -494,10 +494,15 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
 
   alwaysOnLateralActive = scene.always_on_lateral_active;
   showAlwaysOnLateralStatusBar = scene.show_aol_status_bar;
-  if (showAlwaysOnLateralStatusBar) {
+  if (showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar) {
     drawStatusBar(painter);
   }
 
+  conditionalSpeed = scene.conditional_speed;
+  conditionalSpeedLead = scene.conditional_speed_lead;
+  conditionalStatus = scene.conditional_status;
+  showConditionalExperimentalStatusBar = scene.show_cem_status_bar;
+
   experimentalMode = scene.experimental_mode;
 
   mapOpen = scene.map_open;
@@ -528,8 +533,23 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.setOpacity(1.0);
   p.drawRoundedRect(statusBarRect, 30, 30);
 
+  static const std::map<int, QString> conditionalStatusMap = {
+    {0, tr("Conditional Experimental Mode ready")},
+    {7, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeedLead) + leadSpeedUnit)},
+    {8, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeed) + leadSpeedUnit)},
+    {9, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))},
+    {10, tr("Experimental Mode activated for intersection")},
+    {11, tr("Experimental Mode activated for upcoming turn")},
+    {12, tr("Experimental Mode activated for curve")},
+    {13, tr("Experimental Mode activated for slower lead")},
+    {14, tr("Experimental Mode activated for stopped lead")},
+    {15, tr("Experimental Mode activated for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))},
+  };
+
   if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {
     newStatus = tr("Always On Lateral active") + (mapOpen ? "" : tr(". Press the \"Cruise Control\" button to disable"));
+  } else if (showConditionalExperimentalStatusBar) {
+    newStatus = conditionalStatusMap.at(conditionalStatus);
   }
 
   if (newStatus != lastShownStatus) {

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -56,12 +56,16 @@ private:
   bool experimentalMode;
   bool mapOpen;
   bool showAlwaysOnLateralStatusBar;
+  bool showConditionalExperimentalStatusBar;
 
   float accelerationConversion;
   float distanceConversion;
   float speedConversion;
 
   int alertSize;
+  int conditionalSpeed;
+  int conditionalSpeedLead;
+  int conditionalStatus;
 
   QString accelerationUnit;
   QString leadDistanceUnit;

selfdrive/ui/ui.cc

@@ -272,6 +272,11 @@ void ui_update_frogpilot_params(UIState *s) {
   bool always_on_lateral = params.getBool("AlwaysOnLateral");
   scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar");
 
+  scene.conditional_experimental = scene.longitudinal_control && params.getBool("ConditionalExperimental");
+  scene.conditional_speed = scene.conditional_experimental ? params.getInt("CESpeed") : 0;
+  scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0;
+  scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar");
+
   scene.tethering_config = params.getInt("TetheringEnabled");
   if (scene.tethering_config == 2) {
     WifiManager(s).setTetheringEnabled(true);
@@ -354,6 +359,7 @@ void UIState::update() {
   }
 
   // FrogPilot variables that need to be constantly updated
+  scene.conditional_status = scene.conditional_experimental && scene.enabled ? paramsMemory.getInt("CEStatus") : 0;
 }
 
 void UIState::setPrimeType(PrimeType type) {

selfdrive/ui/ui.h

@@ -118,6 +118,7 @@ typedef struct UIScene {
 
   // FrogPilot variables
   bool always_on_lateral_active;
+  bool conditional_experimental;
   bool enabled;
   bool experimental_mode;
   bool map_open;
@@ -125,9 +126,13 @@ typedef struct UIScene {
   bool parked;
   bool right_hand_drive;
   bool show_aol_status_bar;
+  bool show_cem_status_bar;
   bool tethering_enabled;
 
   int alert_size;
+  int conditional_speed;
+  int conditional_speed_lead;
+  int conditional_status;
   int tethering_config;
 
 } UIScene;