retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Remove slow down for turns (#1647) 

* Remove slow down for turns

* update ref
albatross
Willem Melching 2020-06-05 16:01:53 -07:00 committed by GitHub
parent 8c346dfae5
commit 15dc6044d4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 2 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
selfdrive/controls/lib/planner.py
View File

@ -78,8 +78,6 @@ class Planner():
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
@ -90,7 +88,7 @@ class Planner():
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'model': self.v_model, 'cruise': self.v_cruise}
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
@ -109,9 +107,6 @@ class Planner():
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.v_model
self.a_acc = self.a_model
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
@ -133,24 +128,6 @@ class Planner():
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if len(sm['model'].path.poly):
path = list(sm['model'].path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled and not self.first_loop:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
@ -168,12 +145,6 @@ class Planner():
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
model_speed,
2*accel_limits[1], accel_limits[0],
2*jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
5abe4f99eec3633bc30fda47c272e0f19a840a29
0533f640ab27f7b5af57aa4ebf4a29200550b3e8