From d79ca459567ef1dd3c6395eb693f204e423b0a0f Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Smoother braking behind lead Added toggle to smooth out the braking behavior when approaching a slower lead vehicle. --- common/params.cc | 1 + .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 11 ++++++++++- selfdrive/frogpilot/functions/frogpilot_planner.py | 1 + selfdrive/frogpilot/ui/control_settings.cc | 1 + selfdrive/frogpilot/ui/control_settings.h | 2 +- 5 files changed, 14 insertions(+), 2 deletions(-) diff --git a/common/params.cc b/common/params.cc index c4df18cce..89f1400fb 100644 --- a/common/params.cc +++ b/common/params.cc @@ -348,6 +348,7 @@ std::unordered_map keys = { {"ShowStorageLeft", PERSISTENT}, {"ShowStorageUsed", PERSISTENT}, {"Sidebar", PERSISTENT}, + {"SmoothBraking", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 48ec7bbc7..0e1c1d37e 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -243,6 +243,7 @@ class LongitudinalMpc: def __init__(self, mode='acc'): # FrogPilot variables self.acceleration_offset = 1 + self.braking_offset = 1 self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -296,7 +297,7 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard): jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality) - jerk_factor /= np.mean(self.acceleration_offset) + jerk_factor /= np.mean(self.acceleration_offset + self.braking_offset) if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 @@ -373,6 +374,14 @@ class LongitudinalMpc: else: self.acceleration_offset = 1 + # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead + if frogpilot_planner.smoother_braking: + distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow)) + self.braking_offset = np.clip((v_ego - lead_xv_0[:,1]) - COMFORT_BRAKE, 1, distance_factor) + t_follow = t_follow / self.braking_offset + else: + self.braking_offset = 1 + # LongitudinalPlan variables for onroad driving insights if self.status: self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow))) diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 5be215ab6..39cb24150 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -159,6 +159,7 @@ class FrogPilotPlanner: self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0 self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration") self.increased_stopping_distance = 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 params.get_bool("SmoothBraking") self.map_turn_speed_controller = params.get_bool("MTSCEnabled") if self.map_turn_speed_controller: diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index a94061bef..4893c07ac 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -30,6 +30,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"DecelerationProfile", "Deceleration Profile", "Change the deceleration rate to be either sporty or eco-friendly.", ""}, {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""}, {"StoppingDistance", "Increase Stop Distance Behind Lead", "Increase the stopping distance for a more comfortable stop from lead vehicles.", ""}, + {"SmoothBraking", "Smoother Braking Behind Lead", "Smoothen out the braking behavior when approaching slower vehicles.", ""}, {"Model", "Model Selector", "Choose your preferred openpilot model.", "../assets/offroad/icon_calibration.png"}, diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 57d8369b3..59f673cb2 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -41,7 +41,7 @@ private: std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"}; std::set speedLimitControllerKeys = {};