diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 080782ad0f..6b79813117 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -301,8 +301,10 @@ class LongitudinalMpc: return lead_xv def set_accel_limits(self, min_a, max_a): + # TODO this sets a max accel limit, but the minimum limit is only for cruise decel + # needs refactor self.cruise_min_a = min_a - self.cruise_max_a = max_a + self.max_a = max_a def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_ego = self.x0[1] @@ -317,16 +319,17 @@ 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[:,1] = self.max_a + # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': - self.params[:,0] = MIN_ACCEL - self.params[:,1] = self.cruise_max_a self.params[:,5] = LEAD_DANGER_FACTOR # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) - v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05) + v_upper = v_ego + (T_IDXS * self.max_a * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) @@ -338,9 +341,6 @@ class LongitudinalMpc: x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0 elif self.mode == 'blended': - self.params[:,0] = MIN_ACCEL - self.params[:,1] = MAX_ACCEL - self.params[:,5] = 1.0 x_obstacles = np.column_stack([lead_0_obstacle, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2fa13bfb15..cae378453c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -10,7 +10,7 @@ from common.params import Params from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longcontrol import LongCtrlState -from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc +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 T_IDXS as T_IDXS_MPC from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N from system.swaglog import cloudlog @@ -103,8 +103,11 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - 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) + if self.mpc.mode == 'acc': + 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_turns = [MIN_ACCEL, MAX_ACCEL] if reset_state: self.v_desired_filter.x = v_ego