diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 46d39b34fb..3769be56d3 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -7,6 +7,7 @@ from common.numpy_fast import clip from system.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import index_function +from selfdrive.car.interfaces import ACCEL_MIN from selfdrive.controls.radard import _LEAD_ACCEL_TAU if __name__ == '__main__': # generating code @@ -52,8 +53,6 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1 T_IDXS = np.array(T_IDXS_LST) FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) -MIN_ACCEL = -3.5 -MAX_ACCEL = 2.0 COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 @@ -316,7 +315,7 @@ class LongitudinalMpc: # MPC will not converge if immediate crash is expected # Clip lead distance to what is still possible to brake for - min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2) + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) x_lead = clip(x_lead, min_x_lead, 1e8) v_lead = clip(v_lead, 0.0, 1e8) a_lead = clip(a_lead, -10., 5.) @@ -343,7 +342,7 @@ class LongitudinalMpc: lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) - self.params[:,0] = MIN_ACCEL + self.params[:,0] = ACCEL_MIN self.params[:,1] = self.max_a # Update in ACC mode or ACC/e2e blend diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a9c3cc7804..b6ac7bec92 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -10,8 +10,9 @@ from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS +from selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from selfdrive.controls.lib.longcontrol import LongCtrlState -from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error from system.swaglog import cloudlog @@ -110,8 +111,8 @@ class LongitudinalPlanner: accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) else: - accel_limits = [MIN_ACCEL, MAX_ACCEL] - accel_limits_turns = [MIN_ACCEL, MAX_ACCEL] + accel_limits = [ACCEL_MIN, ACCEL_MAX] + accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] if reset_state: self.v_desired_filter.x = v_ego