From d86d8ea476060a17e61af8448bfa60030d7a0a0e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 1 Jan 2022 02:35:17 -0700 Subject: [PATCH] Follow plan sooner after engaging (#545) * Set accel 0 when not active * Smooth in a_ego * conditional accel constraint no longer needed comment comment * tune to 0.5 * Make a_desired and v_desired FirstOrderFilters * update releases --- README.md | 2 +- SA_RELEASES.md | 4 +++ .../lib/longitudinal_mpc_lib/long_mpc.py | 8 ++--- .../controls/lib/longitudinal_planner.py | 32 +++++++++---------- .../test/longitudinal_maneuvers/plant.py | 4 +-- 5 files changed, 24 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 05fbfad7a49c1e..c826fd5175bea7 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Stock Additions - [2021-12-28](/SA_RELEASES.md) (0.8.13) +# Stock Additions - [2021-12-31](/SA_RELEASES.md) (0.8.13) Stock Additions is a fork of openpilot designed to be minimal in design while boasting various feature additions and behavior improvements over stock. I have a 2017 Toyota Corolla with comma pedal, so most of my changes are designed to improve the longitudinal performance. diff --git a/SA_RELEASES.md b/SA_RELEASES.md index d93db2db7a8582..1a6ab070cd163e 100644 --- a/SA_RELEASES.md +++ b/SA_RELEASES.md @@ -1,5 +1,9 @@ Stock Additions 0.8.13 === +## - 2021-12-31, 5:00pm MST Notes + * A few controls improvements related to right after engaging: + * Car no longer brakes for a second after engaging at low speeds if there's no lead + * Follows plan sooner: After you take your foot off the gas pedal to accelerate while engaged, the car now brakes much earlier. Previously the car would continue to accelerate for a second even if we're above the set speed or close to a car. ## - 2021-12-28, 3:00am MST Notes * Use correct path offset for comma three, may reduce slight left hugging ## - 2021-12-24, 3:23am MST Notes diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3ccec8adfc42d6..12a0c5d130b616 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -323,10 +323,9 @@ def set_desired_TR(self, desired_TR): self.desired_TR = desired_TR self.set_weights() - def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): + def update(self, carstate, radarstate, v_cruise): self.v_ego = carstate.vEgo 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, 0) @@ -357,10 +356,7 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): 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) - if prev_accel_constraint: - self.params[:,3] = np.copy(self.prev_a) - else: - self.params[:,3] = a_ego + self.params[:,3] = np.copy(self.prev_a) self.params[:, 4] = self.desired_TR self.run() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4af37a2307f199..a0bf6b510c0ebf 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -4,6 +4,7 @@ from common.numpy_fast import interp import cereal.messaging as messaging +from common.filter_simple import FirstOrderFilter from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS from selfdrive.config import Conversions as CV @@ -48,9 +49,8 @@ def __init__(self, CP, init_v=0.0, init_a=0.0): self.fcw = False - self.v_desired = init_v - self.a_desired = init_a - self.alpha = np.exp(-DT_MDL / 2.0) + self.v_desired = FirstOrderFilter(init_v, 2.0, DT_MDL) + self.a_desired = FirstOrderFilter(init_a, 0.5, DT_MDL) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -67,16 +67,14 @@ def update(self, sm): long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel - prev_accel_constraint = True if long_control_state == LongCtrlState.off 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 + self.v_desired.x = v_ego + # Smooth in current a_ego so we always have an accurate plan + self.a_desired.update(a_ego) # Prevent divergence, smooth in current v_ego - self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego - self.v_desired = max(0.0, self.v_desired) + self.v_desired.update(v_ego) + self.v_desired.x = max(0.0, self.v_desired.x) 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) @@ -85,11 +83,11 @@ def update(self, sm): accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) # clip limits, cannot init MPC outside of bounds - accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) - accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) + accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired.x + 0.05) + accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired.x - 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, prev_accel_constraint=prev_accel_constraint) + self.mpc.set_cur_state(self.v_desired.x, self.a_desired.x) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise) 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) @@ -100,9 +98,9 @@ def update(self, sm): cloudlog.info("FCW triggered") # Interpolate 0.05 seconds and save as starting point for next iteration - a_prev = self.a_desired - self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) - self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0 + a_prev = self.a_desired.x + self.a_desired.x = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) + self.v_desired.x = self.v_desired.x + DT_MDL * (self.a_desired.x + a_prev) / 2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index cefc2166ec5c78..f0ca16182c6baa 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -97,8 +97,8 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): 'carState': car_state.carState, 'controlsState': control.controlsState} self.planner.update(sm) - self.speed = self.planner.v_desired - self.acceleration = self.planner.a_desired + self.speed = self.planner.v_desired.x + self.acceleration = self.planner.a_desired.x fcw = self.planner.fcw self.distance_lead = self.distance_lead + v_lead * self.ts