From b72f5748c4546acca19357ac53aa7a07b9307e40 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 12 Jan 2024 22:39:30 -0700 Subject: [PATCH] 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 | 1 + selfdrive/car/gm/carstate.py | 18 ++++++++ selfdrive/car/honda/carstate.py | 15 +++++++ selfdrive/car/hyundai/carstate.py | 32 ++++++++++++++ selfdrive/car/interfaces.py | 3 ++ selfdrive/car/toyota/carstate.py | 16 +++++++ .../conditional_experimental_mode.py | 12 +++++- selfdrive/frogpilot/ui/control_settings.cc | 1 + selfdrive/ui/qt/onroad.cc | 43 +++++++++++++++++-- selfdrive/ui/qt/onroad.h | 3 ++ selfdrive/ui/ui.cc | 1 + selfdrive/ui/ui.h | 1 + 12 files changed, 141 insertions(+), 5 deletions(-) diff --git a/common/params.cc b/common/params.cc index 50a3c07..be3a034 100644 --- a/common/params.cc +++ b/common/params.cc @@ -246,6 +246,7 @@ std::unordered_map keys = { {"DeviceShutdown", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, {"DriverCamera", PERSISTENT}, + {"ExperimentalModeViaPress", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"GoatScream", PERSISTENT}, diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 4c79338..f101fb4 100644 --- a/selfdrive/car/gm/carstate.py +++ b/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 diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 03aedb3..046b255 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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): diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 55c0d7f..43a84f7 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/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 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 717405e..ebecde4 100644 --- a/selfdrive/car/interfaces.py +++ b/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", diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index f580e6c..caa85a6 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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 diff --git a/selfdrive/frogpilot/functions/conditional_experimental_mode.py b/selfdrive/frogpilot/functions/conditional_experimental_mode.py index e37f7a0..338e925 100644 --- a/selfdrive/frogpilot/functions/conditional_experimental_mode.py +++ b/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") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 5aafcf6..320ea21 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/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.", ""}, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 9db7f58..0d34a4a 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -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)); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 0d14af0..af91aad 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/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); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index e8f547c..f0e8e2e 100644 --- a/selfdrive/ui/ui.cc +++ b/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"); } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 4744829..7fc8764 100644 --- a/selfdrive/ui/ui.h +++ b/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;