From b25c86829f66bb0c180cb10d08275e29d7bf1e87 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 26 Apr 2024 00:18:12 -0700 Subject: [PATCH] Pause lateral control below specified speeds Added toggles to pause lateral control when a turn signal is active or below a set speed. --- common/params.cc | 2 ++ selfdrive/controls/controlsd.py | 9 ++++++++- .../frogpilot/ui/qt/offroad/control_settings.cc | 13 +++++++++++++ .../frogpilot/ui/qt/offroad/control_settings.h | 2 +- 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/common/params.cc b/common/params.cc index 11c9e38d2..8520e6404 100644 --- a/common/params.cc +++ b/common/params.cc @@ -343,6 +343,8 @@ std::unordered_map keys = { {"PathEdgeWidth", PERSISTENT}, {"PathWidth", PERSISTENT}, {"PauseAOLOnBrake", PERSISTENT}, + {"PauseLateralOnSignal", PERSISTENT}, + {"PauseLateralSpeed", PERSISTENT}, {"PreferredSchedule", PERSISTENT}, {"PromptDistractedVolume", PERSISTENT}, {"PromptVolume", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d72296692..880ecc73e 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -193,6 +193,7 @@ class Controls: self.holiday_theme_alerted = False self.onroad_distance_pressed = False self.previously_enabled = False + self.speed_check = False self.previous_lead_distance = 0 @@ -602,7 +603,7 @@ class Controls: # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill - CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.joystick_mode) CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl @@ -963,6 +964,7 @@ class Controls: self.FPCC.alwaysOnLateral &= CS.cruiseState.available self.FPCC.alwaysOnLateral &= self.always_on_lateral self.FPCC.alwaysOnLateral &= self.driving_gear + self.FPCC.alwaysOnLateral &= self.speed_check self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill if self.CP.openpilotLongitudinalControl and self.frogpilot_variables.conditional_experimental_mode: @@ -979,6 +981,9 @@ class Controls: self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED self.previously_enabled &= self.driving_gear + signal_check = CS.vEgo >= self.pause_lateral_below_speed or not (CS.leftBlinker or CS.rightBlinker) or CS.standstill + self.speed_check = CS.vEgo >= self.pause_lateral_below_speed or CS.standstill or signal_check and self.pause_lateral_below_signal + fpcc_send = messaging.new_message('frogpilotCarControl') fpcc_send.valid = CS.canValid fpcc_send.frogpilotCarControl = self.FPCC @@ -1024,6 +1029,8 @@ class Controls: quality_of_life = self.params.get_bool("QOLControls") self.frogpilot_variables.custom_cruise_increase = self.params.get_int("CustomCruise") if quality_of_life else 1 self.frogpilot_variables.custom_cruise_increase_long = self.params.get_int("CustomCruiseLong") if quality_of_life else 5 + self.pause_lateral_below_speed = self.params.get_int("PauseLateralSpeed") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) if quality_of_life else 0 + self.pause_lateral_below_signal = quality_of_life and self.params.get_bool("PauseLateralOnSignal") self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise") toyota_doors = self.params.get_bool("ToyotaDoors") diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 8b6c13194..142f80d2c 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -104,6 +104,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"CustomCruiseLong", tr("Cruise Increase Interval (Long Press)"), tr("Set a custom interval to increase the max set speed by when holding down the cruise increase button."), ""}, {"DisableOnroadUploads", tr("Disable Onroad Uploads"), tr("Prevent uploads to comma connect unless you're offroad and connected to Wi-Fi."), ""}, {"OnroadDistanceButton", tr("Onroad Distance Button"), tr("Simulate a distance button via the onroad UI to control personalities, 'Experimental Mode', and 'Traffic Mode'."), ""}, + {"PauseLateralSpeed", tr("Pause Lateral Below"), tr("Pause lateral control on all speeds below the set speed."), ""}, {"ReverseCruise", tr("Reverse Cruise Increase"), tr("Reverses the 'long press' functionality logic to increase the max set speed by 5 instead of 1. Useful to increase the max speed quickly."), ""}, }; @@ -527,6 +528,12 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector onroadDistanceToggles{"KaofuiIcons"}; std::vector onroadDistanceToggleNames{tr("Kaofui's Icons")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, onroadDistanceToggles, onroadDistanceToggleNames); + } else if (param == "PauseLateralSpeed") { + std::vector pauseLateralToggles{"PauseLateralOnSignal"}; + std::vector pauseLateralToggleNames{"Turn Signal Only"}; + toggle = new FrogPilotParamValueToggleControl(param, title, desc, icon, 0, 99, std::map(), this, false, tr(" mph"), 1, 1, pauseLateralToggles, pauseLateralToggleNames); + } else if (param == "PauseLateralOnSignal") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map(), this, false, tr(" mph")); } else if (param == "ReverseCruise") { std::vector reverseCruiseToggles{"ReverseCruiseUI"}; std::vector reverseCruiseNames{tr("Control Via UI")}; @@ -696,6 +703,8 @@ void FrogPilotControlsPanel::updateMetric() { params.putIntNonBlocking("CustomCruiseLong", std::nearbyint(params.getInt("CustomCruiseLong") * speedConversion)); params.putIntNonBlocking("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion)); params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion)); + params.putIntNonBlocking("PauseLateralOnSignal", std::nearbyint(params.getInt("PauseLateralOnSignal") * speedConversion)); + params.putIntNonBlocking("PauseLateralSpeed", std::nearbyint(params.getInt("PauseLateralSpeed") * speedConversion)); params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } @@ -703,6 +712,7 @@ void FrogPilotControlsPanel::updateMetric() { FrogPilotParamValueControl *customCruiseLongToggle = static_cast(toggles["CustomCruiseLong"]); FrogPilotParamValueControl *laneWidthToggle = static_cast(toggles["LaneDetectionWidth"]); FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast(toggles["PauseAOLOnBrake"]); + FrogPilotParamValueControl *pauseLateralToggle = static_cast(toggles["PauseLateralSpeed"]); FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { @@ -710,12 +720,14 @@ void FrogPilotControlsPanel::updateMetric() { customCruiseLongToggle->updateControl(1, 150, tr(" kph")); laneWidthToggle->updateControl(0, 30, tr(" meters"), 10); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" kph")); + pauseLateralToggle->updateControl(0, 99, tr(" kph")); stoppingDistanceToggle->updateControl(0, 5, tr(" meters")); } else { customCruiseToggle->updateControl(1, 99, tr(" mph")); customCruiseLongToggle->updateControl(1, 99, tr(" mph")); laneWidthToggle->updateControl(0, 100, tr(" feet"), 10); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" mph")); + pauseLateralToggle->updateControl(0, 99, tr(" mph")); stoppingDistanceToggle->updateControl(0, 10, tr(" feet")); } @@ -723,6 +735,7 @@ void FrogPilotControlsPanel::updateMetric() { customCruiseLongToggle->refresh(); laneWidthToggle->refresh(); pauseAOLOnBrakeToggle->refresh(); + pauseLateralToggle->refresh(); stoppingDistanceToggle->refresh(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index bc0947285..8b63c76f2 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -45,7 +45,7 @@ private: std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; - std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "ReverseCruise"}; + std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise"}; std::set speedLimitControllerKeys = {}; std::set speedLimitControllerControlsKeys = {}; std::set speedLimitControllerQOLKeys = {};