Experimental Mode via steering wheel / onroad UI

Added toggle to enable or disable Experimental Mode from the steering wheel for Toyota/Lexus vehicles and the onroad UI for other makes.

common/params.cc

@@ -246,6 +246,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DeviceShutdown", PERSISTENT},
     {"DisableOnroadUploads", PERSISTENT},
     {"DriverCamera", PERSISTENT},
+    {"ExperimentalModeViaPress", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"GasRegenCmd", PERSISTENT},
     {"GoatScream", PERSISTENT},

selfdrive/car/gm/carstate.py

@@ -144,6 +144,24 @@ class CarState(CarStateBase):
       ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
       ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
 
+    # Toggle Experimental Mode from steering wheel function
+    if self.experimental_mode_via_press and ret.cruiseState.available:
+      if self.CP.carFingerprint in SDGM_CAR:
+        lkas_pressed = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
+      else:
+        lkas_pressed = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if self.conditional_experimental_mode:
+          # Set "CEStatus" to work with "Conditional Experimental Mode"
+          conditional_status = self.param_memory.get_int("CEStatus")
+          override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
+          self.param_memory.put_int("CEStatus", override_value)
+        else:
+          experimental_mode = self.param.get_bool("ExperimentalMode")
+          # Invert the value of "ExperimentalMode"
+          put_bool_nonblocking("ExperimentalMode", not experimental_mode)
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   @staticmethod

selfdrive/car/honda/carstate.py

@@ -263,6 +263,21 @@ class CarState(CarStateBase):
       ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
       ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
 
+    # Toggle Experimental Mode from steering wheel function
+    if self.experimental_mode_via_press and ret.cruiseState.available:
+      lkas_pressed = self.cruise_setting == 1
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if self.conditional_experimental_mode:
+          # Set "CEStatus" to work with "Conditional Experimental Mode"
+          conditional_status = self.param_memory.get_int("CEStatus")
+          override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
+          self.param_memory.put_int("CEStatus", override_value)
+        else:
+          experimental_mode = self.param.get_bool("ExperimentalMode")
+          # Invert the value of "ExperimentalMode"
+          put_bool_nonblocking("ExperimentalMode", not experimental_mode)
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   def get_can_parser(self, CP):

selfdrive/car/hyundai/carstate.py

@@ -167,6 +167,21 @@ class CarState(CarStateBase):
     if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
       self.main_enabled = not self.main_enabled
 
+    # Toggle Experimental Mode from steering wheel function
+    if self.experimental_mode_via_press and ret.cruiseState.available:
+      lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if self.conditional_experimental_mode:
+          # Set "CEStatus" to work with "Conditional Experimental Mode"
+          conditional_status = self.param_memory.get_int("CEStatus")
+          override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
+          self.param_memory.put_int("CEStatus", override_value)
+        else:
+          experimental_mode = self.param.get_bool("ExperimentalMode")
+          # Invert the value of "ExperimentalMode"
+          put_bool_nonblocking("ExperimentalMode", not experimental_mode)
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   def update_canfd(self, cp, cp_cam):
@@ -252,6 +267,21 @@ class CarState(CarStateBase):
       self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
                                           else cp_cam.vl["CAM_0x2a4"])
 
+    # Toggle Experimental Mode from steering wheel function
+    if self.experimental_mode_via_press and ret.cruiseState.available:
+      lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if self.conditional_experimental_mode:
+          # Set "CEStatus" to work with "Conditional Experimental Mode"
+          conditional_status = self.param_memory.get_int("CEStatus")
+          override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
+          self.param_memory.put_int("CEStatus", override_value)
+        else:
+          experimental_mode = self.param.get_bool("ExperimentalMode")
+          # Invert the value of "ExperimentalMode"
+          put_bool_nonblocking("ExperimentalMode", not experimental_mode)
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   def get_can_parser(self, CP):
@@ -301,6 +331,8 @@ class CarState(CarStateBase):
     else:
       messages.append(("LVR12", 100))
 
+    messages.append(("BCM_PO_11", 50))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod

selfdrive/car/interfaces.py

@@ -366,6 +366,7 @@ class CarStateBase(ABC):
     self.param = Params()
     self.param_memory = Params("/dev/shm/params")
 
+    self.lkas_previously_pressed = False
     self.main_enabled = False
 
   def update_speed_kf(self, v_ego_raw):
@@ -457,6 +458,8 @@ class CarStateBase(ABC):
     return None
 
   def update_frogpilot_params(self, params):
+    self.conditional_experimental_mode = params.get_bool("ConditionalExperimental")
+    self.experimental_mode_via_press = params.get_bool("ExperimentalModeViaPress")
 
 INTERFACE_ATTR_FILE = {
   "FINGERPRINTS": "fingerprints",

selfdrive/car/toyota/carstate.py

@@ -162,6 +162,22 @@ class CarState(CarStateBase):
     if self.CP.carFingerprint != CAR.PRIUS_V:
       self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
 
+    # Toggle Experimental Mode from steering wheel function
+    if self.experimental_mode_via_press and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V:
+      message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
+      lkas_pressed = any(self.lkas_hud.get(key) == 1 for key in message_keys)
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if self.conditional_experimental_mode:
+          # Set "CEStatus" to work with "Conditional Experimental Mode"
+          conditional_status = self.param_memory.get_int("CEStatus")
+          override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
+          self.param_memory.put_int("CEStatus", override_value)
+        else:
+          experimental_mode = self.param.get_bool("ExperimentalMode")
+          # Invert the value of "ExperimentalMode"
+          self.param.put_bool("ExperimentalMode", not experimental_mode)
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   @staticmethod

selfdrive/frogpilot/functions/conditional_experimental_mode.py

@@ -63,15 +63,22 @@ class ConditionalExperimentalMode:
     self.slowing_down_gmac = GenericMovingAverageCalculator()
 
   def update(self, carState, frogpilotNavigation, modelData, mpc, radarState, standstill, v_ego):
+    # 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, modelData, standstill, v_ego)
-    if (not self.experimental_mode and condition_met):
+    if (not self.experimental_mode and condition_met and overridden not in (1, 3)) or overridden in (2, 4):
       self.experimental_mode = True
-    elif (self.experimental_mode and not condition_met):
+    elif (self.experimental_mode and not condition_met and overridden not in (2, 4)) or overridden in (1, 3):
       self.experimental_mode = False
       self.status_value = 0
 
     # Update the onroad status bar
+    self.status_value = overridden if overridden in (1, 2, 3, 4) else self.status_value
     if self.status_value != self.previous_status_value:
       self.previous_status_value = self.status_value
       self.params_memory.put_int("CEStatus", self.status_value)
@@ -188,6 +195,7 @@ class ConditionalExperimentalMode:
   def update_frogpilot_params(self, is_metric, params):
     self.curves = params.get_bool("CECurves")
     self.curves_lead = params.get_bool("CECurvesLead")
+    self.experimental_mode_via_press = params.get_bool("ExperimentalModeViaPress")
     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)
     self.navigation = params.get_bool("CENavigation")

selfdrive/frogpilot/ui/control_settings.cc

@@ -14,6 +14,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
 
     {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"},
     {"DeviceShutdown", "Device Shutdown Timer", "Configure the timer for automatic device shutdown when offroad conserving energy and preventing battery drain.", "../frogpilot/assets/toggle_icons/icon_time.png"},
+    {"ExperimentalModeViaPress", "Experimental Mode Via 'LKAS' Button / Screen", "Toggle Experimental Mode by double-clicking the 'Lane Departure'/'LKAS' button or double tapping screen.\n\nOverrides 'Conditional Experimental Mode'.", "../assets/img_experimental_white.svg"},
 
     {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
     {"AverageCurvature", "Average Desired Curvature", "Use Pfeiferj's distance-based curvature adjustment for improved curve handling.", ""},

selfdrive/ui/qt/onroad.cc

@@ -6,6 +6,7 @@
 #include <memory>
 #include <sstream>
 
+#include <QApplication>
 #include <QDebug>
 #include <QMouseEvent>
 
@@ -66,6 +67,12 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()->
   QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState);
   QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition);
   QObject::connect(uiState(), &UIState::primeChanged, this, &OnroadWindow::primeChanged);
+
+  QObject::connect(&clickTimer, &QTimer::timeout, this, [this]() {
+    clickTimer.stop();
+    QMouseEvent *event = new QMouseEvent(QEvent::MouseButtonPress, timeoutPoint, Qt::LeftButton, Qt::LeftButton, Qt::NoModifier);
+    QApplication::postEvent(this, event);
+  });
 }
 
 void OnroadWindow::updateState(const UIState &s) {
@@ -99,12 +106,31 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   // FrogPilot clickable widgets
   bool widgetClicked = false;
 
+  // If the click wasn't for anything specific, change the value of "ExperimentalMode"
+  if (scene.experimental_mode_via_press && e->pos() != timeoutPoint) {
+    if (clickTimer.isActive()) {
+      clickTimer.stop();
+      if (scene.conditional_experimental) {
+        int override_value = (scene.conditional_status >= 1 && scene.conditional_status <= 4) ? 0 : scene.conditional_status >= 5 ? 3 : 4;
+        paramsMemory.putIntNonBlocking("CEStatus", override_value);
+      } else {
+        bool experimentalMode = params.getBool("ExperimentalMode");
+        params.putBoolNonBlocking("ExperimentalMode", !experimentalMode);
+      }
+    } else {
+      clickTimer.start(500);
+    }
+    widgetClicked = true;
+  }
+
 #ifdef ENABLE_MAPS
   if (map != nullptr && !widgetClicked) {
     // Switch between map and sidebar when using navigate on openpilot
     bool sidebarVisible = geometry().x() > 0;
     bool show_map = uiState()->scene.navigate_on_openpilot ? sidebarVisible : !sidebarVisible;
-    map->setVisible(show_map && !map->isVisible());
+    if (!scene.experimental_mode_via_press || map->isVisible()) {
+      map->setVisible(show_map && !map->isVisible());
+    }
   }
 #endif
   // propagation event to parent(HomeWindow)
@@ -303,9 +329,14 @@ void ExperimentalButton::changeMode() {
   Params paramsMemory = Params("/dev/shm/params");
 
   const auto cp = (*uiState()->sm)["carParams"].getCarParams();
-  bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
+  bool can_change = hasLongitudinalControl(cp) && (params.getBool("ExperimentalModeConfirmed") || scene.experimental_mode_via_press);
   if (can_change) {
-    params.putBool("ExperimentalMode", !experimental_mode);
+    if (scene.conditional_experimental) {
+      int override_value = (scene.conditional_status >= 1 && scene.conditional_status <= 4) ? 0 : scene.conditional_status >= 5 ? 3 : 4;
+      paramsMemory.putIntNonBlocking("ConditionalStatus", override_value);
+    } else {
+      params.putBoolNonBlocking("ExperimentalMode", !experimental_mode);
+    }
   }
 }
 
@@ -1227,6 +1258,9 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
     {12, "Experimental Mode activated for stop" + (mapOpen ? "" : QString(" sign / stop light"))},
   };
 
+  QString screenSuffix = ". Double tap the screen to revert";
+  QString wheelSuffix = ". Double press the \"LKAS\" button to revert";
+
   if (alwaysOnLateral) {
     newStatus = QString("Always On Lateral active") + (mapOpen ? "" : ". Press the \"Cruise Control\" button to disable");
   } else if (conditionalExperimental) {
@@ -1241,6 +1275,9 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) {
     displayStatusText = false;
   }
+  if (!alwaysOnLateral && !mapOpen && status != STATUS_DISENGAGED && !newStatus.isEmpty()) {
+    newStatus += (conditionalStatus == 3 || conditionalStatus == 4) ? screenSuffix : (conditionalStatus == 1 || conditionalStatus == 2) ? wheelSuffix : "";
+  }
 
   // Configure the text
   p.setFont(InterFont(40, QFont::Bold));

selfdrive/ui/qt/onroad.h

@@ -200,6 +200,9 @@ private:
   // FrogPilot variables
   UIScene &scene;
 
+  QPoint timeoutPoint = QPoint(420, 69);
+  QTimer clickTimer;
+
 private slots:
   void offroadTransition(bool offroad);
   void primeChanged(bool prime);

selfdrive/ui/ui.cc

@@ -305,6 +305,7 @@ void ui_update_params(UIState *s) {
   scene.unlimited_road_ui_length = scene.model_ui && params.getBool("UnlimitedLength");
 
   scene.driver_camera = params.getBool("DriverCamera");
+  scene.experimental_mode_via_press = params.getBool("ExperimentalModeViaPress");
   scene.screen_brightness = params.getInt("ScreenBrightness");
   scene.wheel_icon = params.getInt("WheelIcon");
 }

selfdrive/ui/ui.h

@@ -182,6 +182,7 @@ typedef struct UIScene {
   bool driver_camera;
   bool enabled;
   bool experimental_mode;
+  bool experimental_mode_via_press;
   bool lead_info;
   bool map_open;
   bool model_ui;