|
|
@ -19,13 +19,18 @@ from selfdrive.swaglog import cloudlog |
|
|
|
LON_MPC_STEP = 0.2 # first step is 0.2s |
|
|
|
LON_MPC_STEP = 0.2 # first step is 0.2s |
|
|
|
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted |
|
|
|
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted |
|
|
|
A_CRUISE_MIN = -1.2 |
|
|
|
A_CRUISE_MIN = -1.2 |
|
|
|
A_CRUISE_MAX = 1.2 |
|
|
|
A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8] |
|
|
|
|
|
|
|
A_CRUISE_MAX_BP = [0., 15., 40.] |
|
|
|
|
|
|
|
|
|
|
|
# Lookup table for turns |
|
|
|
# Lookup table for turns |
|
|
|
_A_TOTAL_MAX_V = [1.7, 3.2] |
|
|
|
_A_TOTAL_MAX_V = [1.7, 3.2] |
|
|
|
_A_TOTAL_MAX_BP = [20., 40.] |
|
|
|
_A_TOTAL_MAX_BP = [20., 40.] |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_max_accel(v_ego): |
|
|
|
|
|
|
|
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): |
|
|
|
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): |
|
|
|
""" |
|
|
|
""" |
|
|
|
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration |
|
|
|
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration |
|
|
@ -85,7 +90,7 @@ class Planner(): |
|
|
|
self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * 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 = max(0.0, self.v_desired) |
|
|
|
|
|
|
|
|
|
|
|
accel_limits = [A_CRUISE_MIN, A_CRUISE_MAX] |
|
|
|
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) |
|
|
|
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) |
|
|
|
if force_slow_decel: |
|
|
|
if force_slow_decel: |
|
|
|
# if required so, force a smooth deceleration |
|
|
|
# if required so, force a smooth deceleration |
|
|
|