diff --git a/openpilot/selfdrive/car/tests/test_cruise_speed.py b/openpilot/selfdrive/car/tests/test_cruise_speed.py index 57e89a11176289..05df5d626ca112 100644 --- a/openpilot/selfdrive/car/tests/test_cruise_speed.py +++ b/openpilot/selfdrive/car/tests/test_cruise_speed.py @@ -41,7 +41,7 @@ def test_cruise_speed(self): cruise_speed = float(self.speed) simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality) - assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' + assert simulation_steady_state == pytest.approx(cruise_speed, abs=1.5), f'Did not reach {self.speed} m/s' # TODO: test pcmCruise diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 02a424c4c21b27..af8e87c05605c9 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -23,7 +23,7 @@ JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource -MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise) +MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1) X_DIM = 3 U_DIM = 1 @@ -55,8 +55,6 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -CRUISE_MIN_ACCEL = -1.2 -CRUISE_MAX_ACCEL = 1.6 MIN_X_LEAD_FACTOR = 0.5 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): @@ -313,9 +311,8 @@ def process_lead(self, lead): lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) - v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) @@ -327,15 +324,7 @@ def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.s 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]) - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) - # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) - - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) self.source = MPC_SOURCES[np.argmin(x_obstacles[0])] self.yref[:,:] = 0.0 diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index a2547f99bfbf78..0dccb689b39d24 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -21,6 +21,10 @@ ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 +# Cruise speed limit (moved out of the MPC) +CRUISE_MAX_ACCEL = 1.6 +CRUISE_JERK = 0.5 # m/s^3, max rate of change of cruise limit acceleration + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -55,6 +59,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) + self.a_cruise = 0.0 self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] self.output_a_target = 0.0 self.output_should_stop = False @@ -130,7 +135,7 @@ def update(self, sm): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -149,6 +154,20 @@ def update(self, sm): action_t = self.CP.longitudinalActuatorDelay + DT_MDL output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + + # Apply cruise speed limit outside the MPC + v_err = v_cruise - v_ego + if force_slow_decel: + a_cruise_target = ACCEL_MIN + elif v_err > 0: + a_cruise_target = CRUISE_MAX_ACCEL * np.clip(v_err / (CRUISE_MAX_ACCEL**2 / (2 * CRUISE_JERK)), 0, 1) + else: + a_cruise_target = 0.0 + self.a_cruise = float(np.clip(a_cruise_target, self.a_cruise - CRUISE_JERK * self.dt, self.a_cruise + CRUISE_JERK * self.dt)) + if self.a_cruise < output_a_target_mpc: + output_a_target_mpc = self.a_cruise + self.mpc.source = LongitudinalPlanSource.cruise + output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop