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.

Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>

selfdrive/controls/controlsd.py

@@ -690,6 +690,10 @@ class Controls:
     # FrogPilot functions
     frogpilot_plan = self.sm['frogpilotPlan']
 
+    # Update Experimental Mode
+    if self.frogpilot_variables.conditional_experimental_mode:
+      self.experimental_mode = frogpilot_plan.conditionalExperimental
+
     # Gear Check
     self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
 
@@ -984,7 +988,11 @@ 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:
+         if not self.frogpilot_variables.conditional_experimental_mode:
+           self.experimental_mode = self.params.get_bool("ExperimentalMode")
+      else:
+        self.experimental_mode = False
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
       time.sleep(0.1)
@@ -1006,6 +1014,8 @@ class Controls:
       t.join()
 
   def update_frogpilot_params(self):
+    self.frogpilot_variables.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
+
     custom_alerts = self.params.get_bool("CustomAlerts")
 
     lateral_tune = self.params.get_bool("LateralTune")

selfdrive/controls/lib/drive_helpers.py

@@ -133,7 +133,10 @@ class VCruiseHelper:
     if self.CP.pcmCruise:
       return
 
-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
+    if frogpilot_variables.conditional_experimental_mode:
+      initial = V_CRUISE_INITIAL
+    else:
+      initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if 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/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -337,6 +337,7 @@ class LongitudinalMpc:
 
   def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(personality)
+    self.t_follow = t_follow
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status