mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-15 14:22:11 +08:00
Code Issues Packages Projects Releases Wiki Activity

Smoother braking behind lead

Browse Source 
Added toggle to smooth out the braking behavior when approaching a slower lead vehicle.
This commit is contained in:
FrogAi
2024-03-21 03:20:03 -07:00
parent d2c5c3c0ff
commit 70051f8f80
4 changed files with 32 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+3
View File
@@ -383,6 +383,9 @@ std::unordered_map<std::string, uint32_t> keys = {
{"ShowStorageUsed", PERSISTENT},
{"ShowTuning", PERSISTENT},
{"Sidebar", PERSISTENT},
{"SmoothBraking", PERSISTENT},
{"SmoothBrakingFarLead", PERSISTENT},
{"SmoothBrakingJerk", PERSISTENT},
{"StandardFollow", PERSISTENT},
{"StandardJerk", PERSISTENT},
{"StandbyMode", PERSISTENT},
selfdrive/frogpilot/controls/frogpilot_planner.py
+14 -2
View File
@@ -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")
selfdrive/frogpilot/ui/qt/offroad/control_settings.cc
+13 -1
View File
@@ -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<std::tuple<QString, QString, QString, QString>> 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<int, QString>(), this, false, tr(" feet"));
} else if (param == "SmoothBraking" && !isRelease) {
std::vector<QString> brakingToggles{"SmoothBrakingJerk", "SmoothBrakingFarLead"};
std::vector<QString> brakingToggleNames{tr("Apply to Jerk"), tr("Far Lead Offset")};
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, brakingToggles, brakingToggleNames);
QObject::connect(static_cast<FrogPilotParamToggleControl*>(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);
selfdrive/frogpilot/ui/qt/offroad/control_settings.h
+2 -2
View File
@@ -43,7 +43,7 @@ private:
std::set<QString> experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"};
std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"};
std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance"};
std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"};
std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"};
std::set<QString> speedLimitControllerKeys = {};
@@ -64,7 +64,7 @@ private:
bool hasPCMCruise;
bool hasDashSpeedLimits;
bool isMetric = params.getBool("IsMetric");
bool isStaging;
bool isRelease;
bool isToyota;
bool started;
};