|
|
|
@ -7,6 +7,7 @@ from common.numpy_fast import clip |
|
|
|
|
from system.swaglog import cloudlog |
|
|
|
|
# WARNING: imports outside of constants will not trigger a rebuild |
|
|
|
|
from selfdrive.modeld.constants import index_function |
|
|
|
|
from selfdrive.car.interfaces import ACCEL_MIN |
|
|
|
|
from selfdrive.controls.radard import _LEAD_ACCEL_TAU |
|
|
|
|
|
|
|
|
|
if __name__ == '__main__': # generating code |
|
|
|
@ -52,8 +53,6 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1 |
|
|
|
|
T_IDXS = np.array(T_IDXS_LST) |
|
|
|
|
FCW_IDXS = T_IDXS < 5.0 |
|
|
|
|
T_DIFFS = np.diff(T_IDXS, prepend=[0.]) |
|
|
|
|
MIN_ACCEL = -3.5 |
|
|
|
|
MAX_ACCEL = 2.0 |
|
|
|
|
COMFORT_BRAKE = 2.5 |
|
|
|
|
STOP_DISTANCE = 6.0 |
|
|
|
|
|
|
|
|
@ -316,7 +315,7 @@ class LongitudinalMpc: |
|
|
|
|
|
|
|
|
|
# MPC will not converge if immediate crash is expected |
|
|
|
|
# Clip lead distance to what is still possible to brake for |
|
|
|
|
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2) |
|
|
|
|
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) |
|
|
|
|
x_lead = clip(x_lead, min_x_lead, 1e8) |
|
|
|
|
v_lead = clip(v_lead, 0.0, 1e8) |
|
|
|
|
a_lead = clip(a_lead, -10., 5.) |
|
|
|
@ -343,7 +342,7 @@ 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[:,0] = ACCEL_MIN |
|
|
|
|
self.params[:,1] = self.max_a |
|
|
|
|
|
|
|
|
|
# Update in ACC mode or ACC/e2e blend |
|
|
|
|