40 lines
1.6 KiB
Python
40 lines
1.6 KiB
Python
import cereal.messaging as messaging
|
|
import numpy as np
|
|
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.numpy_fast import clip, interp
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, V_CRUISE_MAX
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
|
|
from openpilot.selfdrive.modeld.constants import ModelConstants
|
|
|
|
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
|
|
from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
|
|
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
|
|
|
# class FrogPilotPlanner:
|
|
# def __init__(self, params, params_memory):
|
|
# self.params_memory = params_memory
|
|
# self.cem = ConditionalExperimentalMode()
|
|
# self.mtsc = MapTurnSpeedController()
|
|
|
|
# self.override_slc = False
|
|
|
|
# self.overridden_speed = 0
|
|
# self.mtsc_target = 0
|
|
# self.slc_target = 0
|
|
# self.v_cruise = 0
|
|
# self.vtsc_target = 0
|
|
|
|
# self.x_desired_trajectory = np.zeros(CONTROL_N)
|
|
|
|
# self.update_frogpilot_params(params, params_memory)
|
|
|
|
# def update(self, sm, mpc):
|
|
# carState, controlsState, modelData = sm['carState'], sm['controlsState'], sm['modelV2']
|
|
|
|
# enabled = controlsState.enabled
|
|
|
|
# v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX)
|
|
# v_cruise = v_cruise_kph * CV.KPH_TO_MS
|
|
# v_ego = carState.vEgo |