diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 577651f8d4..33b996a59c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -298,8 +298,9 @@ class LongitudinalMpc(): self.cruise_min_a = min_a self.cruise_max_a = max_a - def update(self, carstate, radarstate, v_cruise): + def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): v_ego = self.x0[1] + a_ego = self.x0[2] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) @@ -327,7 +328,10 @@ class LongitudinalMpc(): x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) - self.params[:,3] = np.copy(self.prev_a) + if prev_accel_constraint: + self.params[:,3] = np.copy(self.prev_a) + else: + self.params[:,3] = a_ego self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 80c8f3e6d7..1c1ab0e17b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -67,10 +67,13 @@ class Planner: long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel + prev_accel_constraint = True enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) if not enabled or sm['carState'].gasPressed: self.v_desired = v_ego self.a_desired = a_ego + # Smoothly changing between accel trajectory is only relevant when OP is driving + prev_accel_constraint = False # Prevent divergence, smooth in current v_ego self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego @@ -87,7 +90,7 @@ class Planner: accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired, self.a_desired) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 44523319ee..95bf325d7c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9cbef406393a83b35a8f25aa75099da8f8d68276 \ No newline at end of file +6da38baefb2741902f8b10fbf2b5e64fd15ced1f \ No newline at end of file