mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-29 23:43:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Fix low-speed allow_throttle behavior in long planner (#33894)"

Browse Source
This commit is contained in:
Jason Wen
2025-01-06 19:28:39 -05:00
parent 74dca2fccf
commit b2b7d21b7b
2 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+1 -3
View File
@@ -347,8 +347,7 @@ class LongitudinalMpc:
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
self.params[:,0] = ACCEL_MIN
# negative accel constraint causes problems because negative speed is not allowed
self.params[:,1] = max(0.0, self.max_a)
self.params[:,1] = self.max_a
# Update in ACC mode or ACC/e2e blend
if self.mode == 'acc':
@@ -357,7 +356,6 @@ 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,
selfdrive/controls/lib/longitudinal_planner.py
+5 -5
View File
@@ -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
MIN_ALLOW_THROTTLE_SPEED = 2.5
ACCEL_LIMIT_MARGIN = 0.05
# 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 <= MIN_ALLOW_THROTTLE_SPEED
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= 5.0
if not self.allow_throttle:
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)
# 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)
if force_slow_decel:
v_cruise = 0.0