Smoother braking behind lead

Added toggle to smooth out the braking behavior when approaching a slower lead vehicle.

common/params.cc

@@ -316,6 +316,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ShowStorageUsed", PERSISTENT},
     {"Sidebar", PERSISTENT},
     {"SilentMode", PERSISTENT},
+    {"SmoothBraking", PERSISTENT},
     {"StandardFollow", PERSISTENT},
     {"StandardJerk", PERSISTENT},
     {"StoppingDistance", PERSISTENT},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -351,7 +351,7 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, increased_stopping_distance, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, smoother_braking, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, increased_stopping_distance, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality)
     self.t_follow = t_follow
     v_ego = self.x0[1]
@@ -366,6 +366,12 @@ class LongitudinalMpc:
       t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE + increased_stopping_distance - v_ego), 1, distance_factor)
       t_follow = t_follow / t_follow_offset
 
+    # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
+    if smoother_braking:
+      distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow))
+      t_follow_offset = np.clip((v_ego - lead_xv_0[:,1]) - COMFORT_BRAKE, 1, distance_factor)
+      t_follow = t_follow / t_follow_offset
+
     # LongitudinalPlan variables for onroad driving insights
     if self.status:
       self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow)))

selfdrive/controls/lib/longitudinal_planner.py

@@ -139,7 +139,7 @@ class LongitudinalPlanner:
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, frogpilot_planner.increased_stopping_distance,
+    self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, frogpilot_planner.increased_stopping_distance, frogpilot_planner.smoother_braking,
                     frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality=self.personality)
 
     self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -155,6 +155,7 @@ class FrogPilotPlanner:
     self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0
     self.aggressive_acceleration = params.get_bool("AggressiveAcceleration") and longitudinal_tune
     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 = params.get_bool("SmoothBraking") and longitudinal_tune
 
     self.map_turn_speed_controller = params.get_bool("MTSCEnabled")
 

selfdrive/frogpilot/ui/control_settings.cc

@@ -31,6 +31,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"AccelerationProfile", "Acceleration Profile", "Change the acceleration 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", "Increased Stopping Distance", "Increase the stopping distance for a more comfortable stop.", ""},
+    {"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"},
     {"MTSCEnabled", "Map Turn Speed Control", "Slow down for anticipated curves detected by your downloaded maps.", "../frogpilot/assets/toggle_icons/icon_speed_map.png"},
@@ -259,7 +260,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
   fireTheBabysitterKeys = {"NoLogging", "MuteDM", "MuteDoor", "MuteOverheated", "MuteSeatbelt"};
   laneChangeKeys = {"LaneChangeTime", "LaneDetection", "OneLaneChange", "PauseLateralOnSignal"};
   lateralTuneKeys = {"AverageCurvature", "NNFF"};
-  longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "StoppingDistance"};
+  longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
   speedLimitControllerKeys = {};
   visionTurnControlKeys = {};