diff --git a/common/params.cc b/common/params.cc index f8ff09c..5de1b3f 100644 --- a/common/params.cc +++ b/common/params.cc @@ -365,6 +365,7 @@ std::unordered_map keys = { {"ShowStorageLeft", PERSISTENT}, {"ShowStorageUsed", PERSISTENT}, {"Sidebar", PERSISTENT}, + {"SmoothBraking", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StandbyMode", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 48ec7bb..0e1c1d3 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 e59e7d6..0d88eaa 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -164,6 +164,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 5d7944c..1ee099d 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -37,6 +37,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 34ad074..0a9a9dd 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -43,7 +43,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 = {};