From 70051f8f80202621add470aaa4d24c0e0a070652 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Thu, 21 Mar 2024 03:20:03 -0700 Subject: [PATCH] Smoother braking behind lead Added toggle to smooth out the braking behavior when approaching a slower lead vehicle. --- common/params.cc | 3 +++ .../frogpilot/controls/frogpilot_planner.py | 16 ++++++++++++++-- .../frogpilot/ui/qt/offroad/control_settings.cc | 14 +++++++++++++- .../frogpilot/ui/qt/offroad/control_settings.h | 4 ++-- 4 files changed, 32 insertions(+), 5 deletions(-) diff --git a/common/params.cc b/common/params.cc index 56d39055c..e7bbcc10d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -383,6 +383,9 @@ std::unordered_map keys = { {"ShowStorageUsed", PERSISTENT}, {"ShowTuning", PERSISTENT}, {"Sidebar", PERSISTENT}, + {"SmoothBraking", PERSISTENT}, + {"SmoothBrakingFarLead", PERSISTENT}, + {"SmoothBrakingJerk", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StandbyMode", PERSISTENT}, diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 56f09641b..f0898cb84 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -18,7 +18,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, from openpilot.system.version import get_short_branch from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode -from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, STAGING_BRANCHES, calculate_lane_width, calculate_road_curvature +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS @@ -57,7 +57,7 @@ class FrogPilotPlanner: self.lead_one = Lead() self.mtsc = MapTurnSpeedController() - self.staging = get_short_branch() in STAGING_BRANCHES + self.release = get_short_branch() == "FrogPilot" self.radarless_model = self.params.get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS @@ -146,6 +146,15 @@ class FrogPilotPlanner: acceleration_offset = np.clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor) t_follow /= acceleration_offset + # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead + if self.smoother_braking: + distance_factor = np.maximum(1, lead_distance - (v_lead * t_follow)) + far_lead_offset = max(lead_distance - (v_ego * t_follow) - stopping_distance, 0) if self.smoother_braking_far_lead else 0 + braking_offset = np.clip((v_ego - v_lead) + far_lead_offset - COMFORT_BRAKE, 1, distance_factor) + if self.smoother_braking_jerk: + jerk *= np.minimum(braking_offset, COMFORT_BRAKE) + t_follow /= braking_offset + return jerk, t_follow def update_v_cruise(self, carState, controlsState, enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego): @@ -231,6 +240,9 @@ class FrogPilotPlanner: self.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0 self.aggressive_acceleration = longitudinal_tune and self.params.get_bool("AggressiveAcceleration") self.increased_stopping_distance = self.params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0 + self.smoother_braking = longitudinal_tune and self.params.get_bool("SmoothBraking") + self.smoother_braking_far_lead = self.smoother_braking and self.params.get_bool("SmoothBrakingFarLead") and not self.release + self.smoother_braking_jerk = self.smoother_braking and self.params.get_bool("SmoothBrakingJerk") and not self.release self.map_turn_speed_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("MTSCEnabled") self.mtsc_curvature_check = self.map_turn_speed_controller and self.params.get_bool("MTSCCurvatureCheck") diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 701b0e701..c1f04bc9a 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -44,7 +44,7 @@ bool checkNNFFLogFileExists(const std::string &carFingerprint) { FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { std::string branch = params.get("GitBranch"); - isStaging = branch == "FrogPilot-Development" || branch == "FrogPilot-Staging" || branch == "FrogPilot-Testing"; + isRelease = branch == "FrogPilot"; const std::vector> controlToggles { {"AlwaysOnLateral", tr("Always on Lateral"), tr("Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button."), "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"}, @@ -86,6 +86,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"DecelerationProfile", tr("Deceleration Profile"), tr("Change the deceleration rate to be either sporty or eco-friendly."), ""}, {"AggressiveAcceleration", tr("Increase Acceleration Behind Faster Lead"), tr("Increase aggressiveness when following a faster lead."), ""}, {"StoppingDistance", tr("Increase Stop Distance Behind Lead"), tr("Increase the stopping distance for a more comfortable stop from lead vehicles."), ""}, + {"SmoothBraking", tr("Smoother Braking"), tr("Smoothen out the braking behavior when approaching slower vehicles."), ""}, {"MTSCEnabled", tr("Map Turn Speed Control"), tr("Slow down for anticipated curves detected by the downloaded maps."), "../frogpilot/assets/toggle_icons/icon_speed_map.png"}, {"DisableMTSCSmoothing", tr("Disable MTSC UI Smoothing"), tr("Disables the smoothing for the requested speed in the onroad UI to show exactly what speed MTSC is currently requesting."), ""}, @@ -299,6 +300,17 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil toggle = profileSelection; } else if (param == "StoppingDistance") { toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, std::map(), this, false, tr(" feet")); + } else if (param == "SmoothBraking" && !isRelease) { + std::vector brakingToggles{"SmoothBrakingJerk", "SmoothBrakingFarLead"}; + std::vector brakingToggleNames{tr("Apply to Jerk"), tr("Far Lead Offset")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, brakingToggles, brakingToggleNames); + QObject::connect(static_cast(toggle), &FrogPilotParamToggleControl::buttonClicked, [this](bool checked) { + if (checked) { + FrogPilotConfirmationDialog::toggleAlert( + tr("WARNING: This is very experimental and may cause the car to not brake or stop safely! Please report any issues in the FrogPilot Discord!"), + tr("I understand the risks."), this); + } + }); } else if (param == "MTSCEnabled") { FrogPilotParamManageControl *mtscToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 2ead4d567..6c95d3ea7 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -43,7 +43,7 @@ private: std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"}; std::set speedLimitControllerKeys = {}; @@ -64,7 +64,7 @@ private: bool hasPCMCruise; bool hasDashSpeedLimits; bool isMetric = params.getBool("IsMetric"); - bool isStaging; + bool isRelease; bool isToyota; bool started; };