From dbf5c71daa58f8115c72db8004b584c7f41cb4e3 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 7 Jun 2024 10:51:58 -0700 Subject: [PATCH] Controls - Longitudinal Tuning - Increase Stop Distance Behind Lead Increase the stopping distance for a more comfortable stop from lead vehicles. --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 9 ++++++--- selfdrive/frogpilot/controls/frogpilot_planner.py | 5 +++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index bb6389179..12fe4b6d0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -16,6 +16,8 @@ if __name__ == '__main__': # generating code else: from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython + from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT + from casadi import SX, vertcat MODEL_NAME = 'long' @@ -325,10 +327,10 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead): + def process_lead(self, lead, increased_stopping_distance=0): v_ego = self.x0[1] if lead is not None and lead.status: - x_lead = lead.dRel + x_lead = lead.dRel - increased_stopping_distance v_lead = lead.vLead a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau @@ -357,8 +359,9 @@ class LongitudinalMpc: def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status + increased_distance = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) - lead_xv_0 = self.process_lead(radarstate.leadOne) + lead_xv_0 = self.process_lead(radarstate.leadOne, increased_distance) lead_xv_1 = self.process_lead(radarstate.leadTwo) # To estimate a safe distance from a moving lead, we calculate how much stopping diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index dd671a970..df200f2e3 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -54,8 +54,9 @@ class FrogPilotPlanner: v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead - lead_distance = self.lead_one.dRel - stopping_distance = STOP_DISTANCE + distance_offset = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) + lead_distance = self.lead_one.dRel - distance_offset + stopping_distance = STOP_DISTANCE + distance_offset if frogpilot_toggles.conditional_experimental_mode and controlsState.enabled: self.cem.update(carState, frogpilotNavigation, self.lead_one, modelData, self.model_length, self.road_curvature, self.slower_lead, self.tracking_lead, v_ego, v_lead, frogpilot_toggles)