mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-14 01:12:06 +08:00
@@ -4,6 +4,7 @@ import time
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
# WARNING: imports outside of constants will not trigger a rebuild
|
||||
from openpilot.selfdrive.modeld.constants import index_function
|
||||
@@ -220,8 +221,9 @@ def gen_long_ocp():
|
||||
|
||||
|
||||
class LongitudinalMpc:
|
||||
def __init__(self, mode='acc'):
|
||||
def __init__(self, mode='acc', dt=DT_MDL):
|
||||
self.mode = mode
|
||||
self.dt = dt
|
||||
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
self.reset()
|
||||
self.source = SOURCES[2]
|
||||
@@ -440,7 +442,7 @@ class LongitudinalMpc:
|
||||
self.a_solution = self.x_sol[:,2]
|
||||
self.j_solution = self.u_sol[:,0]
|
||||
|
||||
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
|
||||
self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
|
||||
|
||||
t = time.monotonic()
|
||||
if self.solution_status != 0:
|
||||
|
||||
@@ -47,7 +47,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
||||
class LongitudinalPlanner:
|
||||
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
|
||||
self.CP = CP
|
||||
self.mpc = LongitudinalMpc()
|
||||
self.mpc = LongitudinalMpc(dt=dt)
|
||||
self.fcw = False
|
||||
self.dt = dt
|
||||
|
||||
|
||||
Reference in New Issue
Block a user