diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index fa6d5542bd..65e1421d77 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -347,7 +347,8 @@ class LongitudinalMpc: lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) self.params[:,0] = ACCEL_MIN - self.params[:,1] = self.max_a + # negative accel constraint causes problems because negative speed is not allowed + self.params[:,1] = max(0.0, self.max_a) # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': @@ -356,6 +357,7 @@ class LongitudinalMpc: # 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) + # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * self.max_a * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5f47cebb1f..f1637d960c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -22,7 +22,7 @@ A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.5 -ACCEL_LIMIT_MARGIN = 0.05 +MIN_ALLOW_THROTTLE_SPEED = 2.5 # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] @@ -151,12 +151,12 @@ class LongitudinalPlanner: self.v_model_error = get_speed_error(sm['modelV2'], v_ego) x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) # Don't clip at low speeds since throttle_prob doesn't account for creep - self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= 5.0 + self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED if not self.allow_throttle: - # MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin - clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN) - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast) + clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) + clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) if force_slow_decel: v_cruise = 0.0 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 7eea44aae6..4bc1ebba8f 100644 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -108,18 +108,6 @@ def create_maneuvers(kwargs): breakpoints=[0.0, 2., 2.01], **kwargs, ), - Maneuver( - "slow to 5m/s with allow_throttle = False and pitch = +0.1", - duration=25., - initial_speed=20., - lead_relevancy=False, - prob_throttle_values=[1., 0., 0.], - cruise_values=[20., 20., 20.], - pitch_values=[0., 0.1, 0.1], - breakpoints=[0.0, 2., 2.01], - ensure_slowdown=True, - **kwargs, - ), Maneuver( "approach slower cut-in car at 20m/s", duration=20., @@ -163,6 +151,20 @@ def create_maneuvers(kwargs): **kwargs, ), ] + if not kwargs['e2e']: + # allow_throttle won't trigger with e2e + maneuvers.append(Maneuver( + "slow to 5m/s with allow_throttle = False and pitch = +0.1", + duration=30., + initial_speed=20., + lead_relevancy=False, + prob_throttle_values=[1., 0., 0.], + cruise_values=[20., 20., 20.], + pitch_values=[0., 0.1, 0.1], + breakpoints=[0.0, 2., 2.01], + ensure_slowdown=True, + **kwargs, + )) if not kwargs['force_decel']: # controls relies on planner commanding to move for stock-ACC resume spamming maneuvers.append(Maneuver( diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 80b8d3ac5c..2f565836fd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -2f17dc637bb685cadc08521c63f26c2f9af604d0 \ No newline at end of file +992ac80ef848afb85562ca24b1c5a3d410aacd05 \ No newline at end of file