brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Experimental Mode via steering wheel / onroad UI

Browse Source 
Added toggle to enable or disable Experimental Mode from the steering wheel for Toyota/Lexus vehicles and the onroad UI for other makes.
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent e29204d23a
commit b72f5748c4
12 changed files with 141 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
common/params.cc
View File

@@ -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},

18
selfdrive/car/gm/carstate.py
View File

@@ -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

15
selfdrive/car/honda/carstate.py
View File

@@ -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):

32
selfdrive/car/hyundai/carstate.py
View File

@@ -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

3
selfdrive/car/interfaces.py
View File

@@ -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",

16
selfdrive/car/toyota/carstate.py
View File

@@ -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

12
selfdrive/frogpilot/functions/conditional_experimental_mode.py
View File

@@ -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")

1
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -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.", ""},

43
selfdrive/ui/qt/onroad.cc
View File

@@ -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));

3
selfdrive/ui/qt/onroad.h
View File

@@ -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);

1
selfdrive/ui/ui.cc
View File

@@ -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");
}

1
selfdrive/ui/ui.h
View File

@@ -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;