mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-24 04:03:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add force decel to e2e long (#26448)

Browse Source 
* add force decel to e2e

* Update longitudinal_planner.py
old-commit-hash: f23296bc87
This commit is contained in:
Harald Schäfer
2022-11-11 10:46:03 -05:00
committed by GitHub
parent d87ea66802
commit dfd47a5f4d
2 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -301,8 +301,10 @@ class LongitudinalMpc:
return lead_xv
def set_accel_limits(self, min_a, max_a):
# TODO this sets a max accel limit, but the minimum limit is only for cruise decel
# needs refactor
self.cruise_min_a = min_a
self.cruise_max_a = max_a
self.max_a = max_a
def update(self, carstate, radarstate, v_cruise, x, v, a, j):
v_ego = self.x0[1]
@@ -317,16 +319,17 @@ class LongitudinalMpc:
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])
self.params[:,0] = MIN_ACCEL
self.params[:,1] = self.max_a
# Update in ACC mode or ACC/e2e blend
if self.mode == 'acc':
self.params[:,0] = MIN_ACCEL
self.params[:,1] = self.cruise_max_a
self.params[:,5] = LEAD_DANGER_FACTOR
# 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)
v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
v_lower,
v_upper)
@@ -338,9 +341,6 @@ class LongitudinalMpc:
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
elif self.mode == 'blended':
self.params[:,0] = MIN_ACCEL
self.params[:,1] = MAX_ACCEL
self.params[:,5] = 1.0
x_obstacles = np.column_stack([lead_0_obstacle,

9
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -10,7 +10,7 @@ from common.params import Params
from common.realtime import DT_MDL
from selfdrive.modeld.constants import T_IDXS
from selfdrive.controls.lib.longcontrol import LongCtrlState
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
from system.swaglog import cloudlog
@@ -103,8 +103,11 @@ class LongitudinalPlanner:
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
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)
if self.mpc.mode == 'acc':
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)
else:
accel_limits_turns = [MIN_ACCEL, MAX_ACCEL]
if reset_state:
self.v_desired_filter.x = v_ego