Experimental Mode activation via steering wheel / onroad UI

Added toggle to enable or disable Experimental Mode from the steering wheel or onroad UI.

selfdrive/frogpilot/functions/conditional_experimental_mode.py

@@ -37,15 +37,22 @@ class ConditionalExperimentalMode:
     lead = radarState.leadOne
     v_lead = lead.vLead
 
+    # Set the value of "overridden"
+    if self.experimental_mode_via_press:
+      overridden = self.params_memory.get_int("CEStatus")
+    else:
+      overridden = 0
+
     # Update Experimental Mode based on the current driving conditions
     condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, stop_distance, v_ego)
-    if (not self.experimental_mode and condition_met) and enabled:
+    if ((not self.experimental_mode and condition_met and overridden not in (1, 3, 5)) or overridden in (2, 4, 6)) and enabled:
       self.experimental_mode = True
-    elif (self.experimental_mode and not condition_met) or not enabled:
+    elif (self.experimental_mode and not condition_met and overridden not in (2, 4, 6)) or overridden in (1, 3, 5) or not enabled:
       self.experimental_mode = False
       self.status_value = 0
 
     # Update the onroad status bar
+    self.status_value = overridden if overridden in (1, 2, 3, 4, 5, 6) else self.status_value
     if self.status_value != self.previous_status_value:
       self.params_memory.put_int("CEStatus", self.status_value)
       self.previous_status_value = self.status_value
@@ -177,6 +184,8 @@ class ConditionalExperimentalMode:
     self.curves = params.get_bool("CECurves")
     self.curves_lead = self.curves and params.get_bool("CECurvesLead")
 
+    self.experimental_mode_via_press = params.get_bool("ExperimentalModeActivation")
+
     self.limit = params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
     self.limit_lead = params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
 

selfdrive/frogpilot/functions/frogpilot_functions.py

@@ -80,6 +80,25 @@ class FrogPilotFunctions:
 
     return min(distance_to_lane, distance_to_road_edge)
 
+  @staticmethod
+  def update_cestatus_distance(self):
+    # Set "CEStatus" to work with "Conditional Experimental Mode"
+    conditional_status = params_memory.get_int("CEStatus")
+    override_value = 0 if conditional_status in (1, 2, 3, 4, 5, 6) else 3 if conditional_status >= 7 else 4
+    params_memory.put_int("CEStatus", override_value)
+
+  @staticmethod
+  def update_cestatus_lkas(self):
+    # Set "CEStatus" to work with "Conditional Experimental Mode"
+    conditional_status = params_memory.get_int("CEStatus")
+    override_value = 0 if conditional_status in (1, 2, 3, 4, 5, 6) else 5 if conditional_status >= 7 else 6
+    params_memory.put_int("CEStatus", override_value)
+
+  def update_experimental_mode(self):
+    experimental_mode = self.params.get_bool("ExperimentalMode")
+    # Invert the value of "ExperimentalMode"
+    self.params.put_bool("ExperimentalMode", not experimental_mode)
+
   @staticmethod
   def road_curvature(modelData, v_ego):
     predicted_velocities = np.array(modelData.velocity.x)