init

selfdrive/selfdrived/selfdrived.py

@@ -546,6 +546,7 @@ class SelfdriveD(CruiseHelper):
 
       self.mads.read_params()
       self.car_events_sp.read_params()
+      CruiseHelper.read_params(self)
       time.sleep(0.1)
 
   def run(self):

selfdrive/ui/qt/onroad/buttons.h

@@ -20,13 +20,12 @@ public:
 
 private:
   void paintEvent(QPaintEvent *event) override;
-  void changeMode();
-
-  Params params;
 
 protected:
   virtual void drawButton(QPainter &p);
+  virtual void changeMode();
 
+  Params params;
   QPixmap engage_img;
   QPixmap experimental_img;
   bool experimental_mode;

selfdrive/ui/sunnypilot/qt/onroad/buttons.cc

@@ -9,11 +9,36 @@
 
 #include <QPainter>
 
+#include "selfdrive/ui/qt/util.h"
+
 ExperimentalButtonSP::ExperimentalButtonSP(QWidget *parent) : ExperimentalButton(parent) {
   QObject::disconnect(uiState(), &UIState::uiUpdate, this, &ExperimentalButton::updateState);
   QObject::connect(uiState(), &UIState::uiUpdate, this, &ExperimentalButtonSP::updateState);
 }
 
+void ExperimentalButtonSP::changeMode() {
+  const auto cp = (*uiState()->sm)["carParams"].getCarParams();
+  bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
+  if (can_change) {
+    bool experimental_mode_new;
+    bool dynamic_experimental_control_new;
+
+    if (!experimental_mode && dynamic_experimental_control) {
+      experimental_mode_new = true;
+      dynamic_experimental_control_new = false;
+    } else if (experimental_mode && !dynamic_experimental_control) {
+      experimental_mode_new = true;
+      dynamic_experimental_control_new = true;
+    } else {
+      experimental_mode_new = false;
+      dynamic_experimental_control_new = false;
+    }
+
+    params.putBool("ExperimentalMode", experimental_mode_new);
+    params.putBool("DynamicExperimentalControl", dynamic_experimental_control_new);
+  }
+}
+
 void ExperimentalButtonSP::updateState(const UIState &s) {
   ExperimentalButton::updateState(s);
   const auto long_plan_sp = (*s.sm)["longitudinalPlanSP"].getLongitudinalPlanSP();

selfdrive/ui/sunnypilot/qt/onroad/buttons.h

@@ -18,6 +18,7 @@ public:
 
 private:
   void drawButton(QPainter &p) override;
+  void changeMode() override;
 
   bool dynamic_experimental_control;
   int dec_mpc_mode;

sunnypilot/selfdrive/car/cruise_helpers.py

@@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details.
 
 from cereal import car, custom
 from opendbc.car import structs
-from openpilot.common.params import Params
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventNameSP = custom.OnroadEventSP.EventName
@@ -16,21 +15,26 @@ DISTANCE_LONG_PRESS = 50
 
 
 class CruiseHelper:
-  def __init__(self, CP: structs.CarParams):
+  def __init__(self, CP: structs.CarParams, selfdrived):
     self.CP = CP
-    self.params = Params()
+    self.selfdrived = selfdrived
+    self.params = self.selfdrived.params
 
     self.button_frame_counts = {ButtonType.gapAdjustCruise: 0}
-    self._experimental_mode = False
     self.experimental_mode_switched = False
+    self.experimental_mode = self.selfdrived.experimental_mode
+    self.dynamic_experimental_control = False
 
-  def update(self, CS, events, experimental_mode) -> None:
+  def read_params(self):
+    self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
+
+  def update(self, CS, events) -> None:
     if self.CP.openpilotLongitudinalControl:
       if CS.cruiseState.available:
         self.update_button_frame_counts(CS)
 
         # toggle experimental mode once on distance button hold
-        self.update_experimental_mode(events, experimental_mode)
+        self.update_experimental_mode(events)
 
   def update_button_frame_counts(self, CS) -> None:
     for button in self.button_frame_counts:
@@ -42,9 +46,23 @@ class CruiseHelper:
       if button in self.button_frame_counts:
         self.button_frame_counts[button] = int(button_event.pressed)
 
-  def update_experimental_mode(self, events, experimental_mode) -> None:
+  def update_experimental_mode(self, events) -> None:
     if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched:
-      self._experimental_mode = not experimental_mode
-      self.params.put_bool_nonblocking("ExperimentalMode", self._experimental_mode)
+      if not self.experimental_mode and not self.dynamic_experimental_control:
+        # State 1 -> 2: Turn on experimental mode only
+        experimental_mode = True
+        dynamic_experimental_control = False
+      elif self.experimental_mode and not self.dynamic_experimental_control:
+        # State 2 -> 3: Keep experimental mode on, turn on DEC
+        experimental_mode = True
+        dynamic_experimental_control = True
+      else:
+        # State 3 -> 1: Turn everything off, so back to chill mode
+        experimental_mode = False
+        dynamic_experimental_control = False
+
+      self.params.put_bool_nonblocking("ExperimentalMode", experimental_mode)
+      self.params.put_bool_nonblocking("DynamicExperimentalControl", dynamic_experimental_control)
+
       events.add(EventNameSP.experimentalModeSwitched)
       self.experimental_mode_switched = True