diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f6d2eba91..9408132c5 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -217,7 +217,7 @@ class LongitudinalMpc: self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() - self.source = log.LongitudinalPlan.LongitudinalPlanSource.cruise + self.source = SOURCES[2] def reset(self): self.solver.reset() @@ -335,14 +335,7 @@ class LongitudinalMpc: cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) - lead_idx = np.argmin(x_obstacles[0]) - match lead_idx: - case 0: - self.source = log.LongitudinalPlan.LongitudinalPlanSource.lead0 - case 1: - self.source = log.LongitudinalPlan.LongitudinalPlanSource.lead1 - case 2: - self.source = log.LongitudinalPlan.LongitudinalPlanSource.cruise + self.source = SOURCES[np.argmin(x_obstacles[0])] self.yref[:,:] = 0.0 for i in range(N): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 47228ada4..c5c03eba1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,14 +3,13 @@ import math import numpy as np import cereal.messaging as messaging -from cereal import log from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.constants import CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, SOURCES from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET @@ -157,7 +156,7 @@ class LongitudinalPlanner: output_a_target = min(output_a_target_e2e, output_a_target_mpc) self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc if output_a_target < output_a_target_mpc: - self.mpc.source = log.LongitudinalPlan.LongitudinalPlanSource.e2e + self.mpc.source = SOURCES[3] else: output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc