Controls - Conditional Experimental Mode

Automatically switches to 'Experimental Mode' under predefined conditions.

release/files_common

@@ -567,6 +567,7 @@ tinygrad_repo/tinygrad/*.py
 
 selfdrive/frogpilot/frogpilot_process.py
 selfdrive/frogpilot/controls/frogpilot_planner.py
+selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
 selfdrive/frogpilot/controls/lib/frogpilot_functions.py
 selfdrive/frogpilot/controls/lib/frogpilot_variables.py
 selfdrive/frogpilot/fleet_manager/fleet_manager.py

selfdrive/controls/controlsd.py

@@ -883,7 +883,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")
@@ -922,6 +923,9 @@ class Controls:
     self.FPCC.alwaysOnLateral &= self.driving_gear
     self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill
 
+    if self.frogpilot_toggles.conditional_experimental_mode:
+      self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental
+
     self.drive_distance += CS.vEgo * DT_CTRL
     self.drive_time += DT_CTRL
 

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
                                                                            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 calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
 
@@ -23,6 +24,8 @@ class FrogPilotPlanner:
   def __init__(self):
     self.params_memory = Params("/dev/shm/params")
 
+    self.cem = ConditionalExperimentalMode()
+
     self.acceleration_jerk = 0
     self.frame = 0
     self.road_curvature = 0
@@ -51,6 +54,9 @@ class FrogPilotPlanner:
     self.road_curvature = calculate_road_curvature(modelData, v_ego)
     self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles)
 
+    if frogpilot_toggles.conditional_experimental_mode:
+      self.cem.update(carState, controlsState.enabled, frogpilotNavigation, lead_distance, self.lead_one, modelData, self.road_curvature, self.slower_lead, v_ego, v_lead, frogpilot_toggles)
+
     self.frame += 1
 
   def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles):
@@ -78,6 +84,8 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk)
     frogpilotPlan.tFollow = float(self.t_follow)
 
+    frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
+
     frogpilotPlan.vCruise = float(self.v_cruise)
 
     pm.send('frogpilotPlan', frogpilot_plan_send)

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -0,0 +1,40 @@
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+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
+
+class ConditionalExperimentalMode:
+  def __init__(self):
+    self.params_memory = Params("/dev/shm/params")
+
+    self.experimental_mode = False
+
+    self.lead_detection_mac = MovingAverageCalculator()
+
+  def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
+    self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles)
+
+    condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles) and enabled
+    self.experimental_mode = condition_met
+
+    self.params_memory.put_int("CEStatus", self.status_value if condition_met else 0)
+
+  def check_conditions(self, carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles):
+    if carState.standstill:
+      self.status_value = 0
+      return self.experimental_mode
+
+    if (not self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit) or (self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit_lead):
+      self.status_value = 11 if self.lead_detected else 12
+      return True
+
+    return False
+
+  def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles):
+    self.lead_detection(lead_status)
+
+  def lead_detection(self, lead_status):
+    self.lead_detection_mac.add_data(lead_status)
+    self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY

selfdrive/ui/qt/offroad/settings.cc

@@ -195,7 +195,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 {