diff --git a/common/params.cc b/common/params.cc index bc3b177af..3347d6dad 100644 --- a/common/params.cc +++ b/common/params.cc @@ -254,6 +254,9 @@ std::unordered_map keys = { {"DriveStats", PERSISTENT}, {"DynamicPathWidth", PERSISTENT}, {"EngageVolume", PERSISTENT}, + {"ExperimentalModeActivation", PERSISTENT}, + {"ExperimentalModeViaLKAS", PERSISTENT}, + {"ExperimentalModeViaScreen", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GoatScream", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 1e60947d2..ee270e664 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -213,6 +213,7 @@ class Panda: FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32 FLAG_HYUNDAI_ALT_LIMITS = 64 FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128 + FLAG_HYUNDAI_LFA_BTN = 256 FLAG_TESLA_POWERTRAIN = 1 FLAG_TESLA_LONG_CONTROL = 2 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index d886fc029..7eae2c980 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -152,6 +152,17 @@ class CarState(CarStateBase): ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 + # Toggle Experimental Mode from steering wheel function + if frogpilot_variables.experimental_mode_via_lkas 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: + self.fpf.lkas_button_function(frogpilot_variables.conditional_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 0d6f945f6..cb19fc1e6 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -263,6 +263,13 @@ 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 frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available: + lkas_pressed = self.cruise_setting == 1 + if lkas_pressed and not self.lkas_previously_pressed: + self.fpf.lkas_button_function(frogpilot_variables.conditional_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 0687a653e..0769f03ad 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -170,6 +170,13 @@ 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 frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.flags & HyundaiFlags.CAN_LFA_BTN: + lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] + if lkas_pressed and not self.lkas_previously_pressed: + self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode) + self.lkas_previously_pressed = lkas_pressed + return ret def update_canfd(self, cp, cp_cam, frogpilot_variables): @@ -255,6 +262,13 @@ 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 frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available: + lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] + if lkas_pressed and not self.lkas_previously_pressed: + self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode) + self.lkas_previously_pressed = lkas_pressed + return ret def get_can_parser(self, CP): @@ -304,6 +318,9 @@ class CarState(CarStateBase): else: messages.append(("LVR12", 100)) + if CP.flags & HyundaiFlags.CAN_LFA_BTN: + messages.append(("BCM_PO_11", 50)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 0df21636a..345eff98f 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -316,6 +316,10 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_SCC_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + if 0x391 in fingerprint[0]: + ret.flags |= HyundaiFlags.CAN_LFA_BTN.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN + if ret.openpilotLongitudinalControl: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG if ret.flags & HyundaiFlags.HYBRID: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 1eefc30e3..a74212533 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -67,6 +67,7 @@ class HyundaiFlags(IntFlag): CANFD_HDA2_ALT_STEERING = 512 HYBRID = 1024 EV = 2048 + CAN_LFA_BTN = 4096 class CAR(StrEnum): diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a5a42de5f..60a4b74af 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -373,6 +373,8 @@ class CarStateBase(ABC): # FrogPilot variables self.fpf = FrogPilotFunctions() + self.lkas_previously_pressed = False + def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 7182ea41f..bfc92cea4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -166,6 +166,15 @@ 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 frogpilot_variables.experimental_mode_via_lkas 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: + self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode) + self.lkas_previously_pressed = lkas_pressed + return ret @staticmethod diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8c9e48f17..e9dd7a1fa 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1023,6 +1023,9 @@ class Controls: frog_sounds = custom_sounds == 1 self.goat_scream = frog_sounds and self.params.get_bool("GoatScream") + experimental_mode_activation = self.params.get_bool("ExperimentalModeActivation") + self.frogpilot_variables.experimental_mode_via_lkas = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaLKAS") + lateral_tune = self.params.get_bool("LateralTune") longitudinal_tune = self.params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/functions/conditional_experimental_mode.py b/selfdrive/frogpilot/functions/conditional_experimental_mode.py index bc860edb1..f62465fb9 100644 --- a/selfdrive/frogpilot/functions/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/functions/conditional_experimental_mode.py @@ -39,15 +39,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, modelData, 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)) or overridden in (2, 4)) 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)) or overridden in (1, 3) 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) 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 @@ -179,6 +186,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) diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py index 3353a3432..bd0636169 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -76,6 +76,18 @@ class FrogPilotFunctions: return min(distance_to_lane, distance_to_road_edge) + @staticmethod + def lkas_button_function(conditional_experimental_mode): + if conditional_experimental_mode: + # 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) else 1 if conditional_status >= 5 else 2 + params_memory.put_int("CEStatus", override_value) + else: + experimental_mode = params.get_bool("ExperimentalMode") + # Invert the value of "ExperimentalMode" + params.put_bool("ExperimentalMode", not experimental_mode) + @staticmethod def road_curvature(modelData, v_ego): predicted_velocities = np.array(modelData.velocity.x) diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index cc1e6db55..41a9f1688 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -13,6 +13,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"}, + {"ExperimentalModeActivation", "Experimental Mode Via", "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"}, @@ -142,6 +143,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 33, shutdownLabels, this, false); + } else if (param == "ExperimentalModeActivation") { + std::vector experimentalModeToggles{"ExperimentalModeViaLKAS", "ExperimentalModeViaScreen"}; + std::vector experimentalModeNames{tr("LKAS Button"), tr("Screen")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, experimentalModeToggles, experimentalModeNames); + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 891322b23..102ca180f 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) { @@ -98,12 +105,33 @@ 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_screen && 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 (!clickTimer.isActive()) { + map->setVisible(show_map && !map->isVisible()); + } } #endif // propagation event to parent(HomeWindow) @@ -234,9 +262,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_screen); 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.putBool("ExperimentalMode", !experimental_mode); + } } } @@ -1152,6 +1185,14 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0]; } + // Append suffix to the status + QString screenSuffix = ". Double tap the screen to revert"; + QString wheelSuffix = ". Double press the \"LKAS\" button to revert"; + + if (!alwaysOnLateral && !mapOpen && status != STATUS_DISENGAGED && !newStatus.isEmpty()) { + newStatus += (conditionalStatus == 3 || conditionalStatus == 4) ? screenSuffix : (conditionalStatus == 1 || conditionalStatus == 2) ? wheelSuffix : ""; + } + // Check if status has changed if (newStatus != lastShownStatus) { displayStatusText = true; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 10656c210..dd290071e 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -201,6 +201,9 @@ private: UIScene &scene; Params paramsMemory{"/dev/shm/params"}; + 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 9f3a39faf..0ff71b516 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -306,6 +306,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.custom_signals = custom_theme ? params.getInt("CustomSignals") : 0; scene.driver_camera = params.getBool("DriverCamera"); + scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation"); scene.model_ui = params.getBool("ModelUI"); scene.dynamic_path_width = scene.model_ui && params.getBool("DynamicPathWidth"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index c06672325..e66f1c0f1 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -182,6 +182,7 @@ typedef struct UIScene { bool dynamic_path_width; bool enabled; bool experimental_mode; + bool experimental_mode_via_screen; bool lead_info; bool map_open; bool model_ui;