cleanup long planner, mpc: unused globals and arguments (#2211)
Co-authored-by: user <email@web.com>pull/2212/head
parent
927ef086f7
commit
72e0ac2de1
|
@ -56,7 +56,7 @@ class LongitudinalMpc():
|
||||||
self.cur_state[0].v_ego = v
|
self.cur_state[0].v_ego = v
|
||||||
self.cur_state[0].a_ego = a
|
self.cur_state[0].a_ego = a
|
||||||
|
|
||||||
def update(self, pm, CS, lead, v_cruise_setpoint):
|
def update(self, pm, CS, lead):
|
||||||
v_ego = CS.vEgo
|
v_ego = CS.vEgo
|
||||||
|
|
||||||
# Setup current mpc state
|
# Setup current mpc state
|
||||||
|
|
|
@ -15,10 +15,7 @@ from selfdrive.controls.lib.fcw import FCWChecker
|
||||||
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
|
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
|
||||||
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||||
|
|
||||||
MAX_SPEED = 255.0
|
|
||||||
|
|
||||||
LON_MPC_STEP = 0.2 # first step is 0.2s
|
LON_MPC_STEP = 0.2 # first step is 0.2s
|
||||||
MAX_SPEED_ERROR = 2.0
|
|
||||||
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
|
||||||
|
|
||||||
# lookup tables VS speed to determine min and max accels in cruise
|
# lookup tables VS speed to determine min and max accels in cruise
|
||||||
|
@ -36,9 +33,6 @@ _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.]
|
||||||
_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.]
|
||||||
|
|
||||||
# 75th percentile
|
|
||||||
SPEED_PERCENTILE_IDX = 7
|
|
||||||
|
|
||||||
|
|
||||||
def calc_cruise_accel_limits(v_ego, following):
|
def calc_cruise_accel_limits(v_ego, following):
|
||||||
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||||
|
@ -162,8 +156,8 @@ class Planner():
|
||||||
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
|
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
|
||||||
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
|
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
|
||||||
|
|
||||||
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
|
self.mpc1.update(pm, sm['carState'], lead_1)
|
||||||
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
|
self.mpc2.update(pm, sm['carState'], lead_2)
|
||||||
|
|
||||||
self.choose_solution(v_cruise_setpoint, enabled)
|
self.choose_solution(v_cruise_setpoint, enabled)
|
||||||
|
|
||||||
|
|
|
@ -61,8 +61,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
|
||||||
mpc.set_cur_state(v_ego, a_ego)
|
mpc.set_cur_state(v_ego, a_ego)
|
||||||
if first: # Make sure MPC is converged on first timestep
|
if first: # Make sure MPC is converged on first timestep
|
||||||
for _ in range(20):
|
for _ in range(20):
|
||||||
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
|
mpc.update(pm, CS.carState, lead)
|
||||||
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
|
mpc.update(pm, CS.carState, lead)
|
||||||
|
|
||||||
# Choose slowest of two solutions
|
# Choose slowest of two solutions
|
||||||
if v_cruise < mpc.v_mpc:
|
if v_cruise < mpc.v_mpc:
|
||||||
|
|
Loading…
Reference in New Issue