mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 16:33:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

long_mpc: use log.capnp source enum instead of list (#37093)

Browse Source
This commit is contained in:
felsager
2026-02-05 15:55:03 -08:00
committed by GitHub
parent 28795d3065
commit 7e959c5a3e
2 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -217,7 +217,7 @@ class LongitudinalMpc:
self.dt = dt
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset()
self.source = SOURCES[2]
self.source = log.LongitudinalPlan.LongitudinalPlanSource.cruise
def reset(self):
self.solver.reset()
@@ -335,7 +335,14 @@ 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])
self.source = SOURCES[np.argmin(x_obstacles[0])]
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.yref[:,:] = 0.0
for i in range(N):

5
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -3,13 +3,14 @@ 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, SOURCES
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
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
@@ -156,7 +157,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 = SOURCES[3]
self.mpc.source = log.LongitudinalPlan.LongitudinalPlanSource.e2e
else:
output_a_target = output_a_target_mpc
self.output_should_stop = output_should_stop_mpc