diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index b83354065f..f9bf15cc04 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -56,7 +56,7 @@ class LongitudinalMpc(): self.cur_state[0].v_ego = v 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 # Setup current mpc state diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 553e5bb6fa..0412df36d3 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -15,10 +15,7 @@ from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX -MAX_SPEED = 255.0 - 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 # 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_BP = [20., 40.] -# 75th percentile -SPEED_PERCENTILE_IDX = 7 - def calc_cruise_accel_limits(v_ego, following): 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.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) - self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) - self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) + self.mpc1.update(pm, sm['carState'], lead_1) + self.mpc2.update(pm, sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 11995c6c78..014b22e962 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -61,8 +61,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0): mpc.set_cur_state(v_ego, a_ego) if first: # Make sure MPC is converged on first timestep for _ in range(20): - mpc.update(pm, CS.carState, lead, v_cruise_setpoint) - mpc.update(pm, CS.carState, lead, v_cruise_setpoint) + mpc.update(pm, CS.carState, lead) + mpc.update(pm, CS.carState, lead) # Choose slowest of two solutions if v_cruise < mpc.v_mpc: