Controls - Conditional Experimental Mode

Automatically switches to 'Experimental Mode' under predefined conditions.

selfdrive/controls/controlsd.py

@@ -680,7 +680,7 @@ class Controls:
 
     self.display_timer -= 1
 
-    FPCC = self.update_frogpilot_variables(CS, self.sm['frogpilotCarState'])
+    FPCC = self.update_frogpilot_variables(CS, self.sm['frogpilotCarState'], self.sm['frogpilotPlan'])
 
     return CC, lac_log, FPCC
 
@@ -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")
@@ -892,7 +893,7 @@ class Controls:
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
 
-  def update_frogpilot_variables(self, CS, frogpilotCarState):
+  def update_frogpilot_variables(self, CS, frogpilotCarState, frogpilotPlan):
     driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
 
     self.always_on_lateral_active |= self.frogpilot_toggles.always_on_lateral_main or CS.cruiseState.enabled
@@ -918,6 +919,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 = 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

@@ -13,6 +13,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
                                                                            get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel
 
+from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY
 
@@ -22,6 +23,8 @@ class FrogPilotPlanner:
   def __init__(self):
     self.params_memory = Params("/dev/shm/params")
 
+    self.cem = ConditionalExperimentalMode(self)
+
     self.tracking_lead = False
 
     self.acceleration_jerk = 0
@@ -44,6 +47,10 @@ class FrogPilotPlanner:
     lead_distance = self.lead_one.dRel
     stopping_distance = STOP_DISTANCE
 
+    run_cem = frogpilot_toggles.conditional_experimental_mode
+    if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear:
+      self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles)
+
     if v_ego >= frogpilot_toggles.minimum_lane_change_speed:
       self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])
       self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])
@@ -111,6 +118,8 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.base_speed_jerk)
     frogpilotPlan.tFollow = float(self.t_follow)
 
+    frogpilotPlan.conditionalExperimentalActive = 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,31 @@
+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, PROBABILITY
+
+class ConditionalExperimentalMode:
+  def __init__(self, FrogPilotPlanner):
+    self.params_memory = Params("/dev/shm/params")
+
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.experimental_mode = False
+
+  def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles):
+    if 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
+
+  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
+    below_speed_with_lead = frogpilot_toggles.conditional_limit_lead > v_ego >= 1 and tracking_lead
+    if below_speed or below_speed_with_lead:
+      self.status_value = 7 if tracking_lead else 8
+      return True
+
+    return False
+
+  def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles):

selfdrive/ui/qt/offroad/settings.cc

@@ -180,7 +180,13 @@ void TogglesPanel::updateToggles() {
     op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable());
     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

@@ -287,7 +287,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
   if (rightHandDM && map_settings_btn->isEnabled()) {
     x -= 250;
   }
-  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);
@@ -497,10 +497,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;
@@ -531,8 +536,23 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.setOpacity(1.0);
   p.drawRoundedRect(statusBarRect, 30, 30);
 
+  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) + (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"))},
+    {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 stopped lead")},
+    {14, tr("Experimental Mode activated for slower 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

@@ -273,6 +273,11 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
   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);
@@ -358,6 +363,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

@@ -119,6 +119,7 @@ typedef struct UIScene {
 
   // FrogPilot variables
   bool always_on_lateral_active;
+  bool conditional_experimental;
   bool enabled;
   bool experimental_mode;
   bool map_open;
@@ -126,9 +127,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;