From 6fb2ad00c3327fbe06720514b73ec9f446bd29c6 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] Aggressive acceleration when following a lead Added toggle to make the acceleration more aggressive when following a lead. --- common/params.cc | 1 + .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 12 ++++++++++++ selfdrive/frogpilot/functions/frogpilot_planner.py | 1 + selfdrive/frogpilot/ui/control_settings.cc | 1 + selfdrive/frogpilot/ui/control_settings.h | 2 +- 5 files changed, 16 insertions(+), 1 deletion(-) diff --git a/common/params.cc b/common/params.cc index 21a1c3e..975e846 100644 --- a/common/params.cc +++ b/common/params.cc @@ -211,6 +211,7 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationPath", PERSISTENT}, {"AccelerationProfile", PERSISTENT}, + {"AggressiveAcceleration", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomUI", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9b2b7d5..67844e8 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -222,6 +222,7 @@ def gen_long_ocp(): class LongitudinalMpc: def __init__(self, mode='acc'): # FrogPilot variables + self.acceleration_offset = 1 self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -275,6 +276,8 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): jerk_factor = get_jerk_factor(personality) + jerk_factor /= np.mean(self.acceleration_offset) + if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] @@ -340,6 +343,15 @@ class LongitudinalMpc: lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) + # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead + if frogpilot_planner.aggressive_acceleration: + distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow)) + standstill_offset = max(STOP_DISTANCE - (v_ego**COMFORT_BRAKE), 0) + self.acceleration_offset = np.clip((lead_xv_0[:,1] - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor) + t_follow = t_follow / self.acceleration_offset + else: + self.acceleration_offset = 1 + # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index bf8e1ab..5bf0a37 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -75,3 +75,4 @@ class FrogPilotPlanner: longitudinal_tune = params.get_bool("LongitudinalTune") self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0 + self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 7f11c13..a983ee8 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -7,6 +7,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""}, {"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.", ""}, {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"}, }; diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 359ab97..a2a3255 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -33,7 +33,7 @@ private: std::set fireTheBabysitterKeys = {}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration"}; std::set mtscKeys = {}; std::set qolKeys = {}; std::set speedLimitControllerKeys = {};