wip

scp/events.py

@@ -0,0 +1,40 @@
+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