Driving Model Selector: Fix lag?

selfdrive/controls/controlsd.py

@@ -211,7 +211,7 @@ class Controls:
 
     self.process_not_running = False
 
-    self.custom_model, self.model_gen = get_model_generation()
+    self.custom_model, self.model_gen = get_model_generation(self.params)
     self.model_use_lateral_planner = self.custom_model and self.model_gen == 1
 
     self.can_log_mono_time = 0

selfdrive/controls/lib/desire_helper.py

@@ -62,7 +62,7 @@ class DesireHelper:
     self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8"))
     self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay")
 
-    self.custom_model, self.model_gen = get_model_generation()
+    self.custom_model, self.model_gen = get_model_generation(self.param_s)
     self.model_use_lateral_planner = self.custom_model and self.model_gen == 1
 
   def read_param(self):

selfdrive/controls/lib/lateral_planner.py

@@ -10,7 +10,6 @@ from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MP
 from openpilot.selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error, get_road_edge
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.sunnypilot import get_model_generation
 
 import cereal.messaging as messaging
 from cereal import log
@@ -30,7 +29,7 @@ STEERING_RATE_COST = 700.0
 
 
 class LateralPlanner:
-  def __init__(self, CP, debug=False):
+  def __init__(self, CP, debug=False, model_use_lateral_planner=False):
     self.LP = LanePlanner()
     self.DH = DesireHelper()
 
@@ -74,8 +73,7 @@ class LateralPlanner:
     self.param_read_counter = 0
     self.read_param()
 
-    self.custom_model, self.model_gen = get_model_generation()
-    self.model_use_lateral_planner = self.custom_model and self.model_gen == 1
+    self.model_use_lateral_planner = model_use_lateral_planner
 
   def read_param(self):
     self.dynamic_lane_profile = int(self.param_s.get("DynamicLaneProfile", encoding='utf8'))

selfdrive/controls/plannerd.py

@@ -15,9 +15,7 @@ import cereal.messaging as messaging
 def cumtrapz(x, t):
   return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
 
-def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
-  custom_model, model_gen = get_model_generation()
-  model_use_lateral_planner = custom_model and model_gen == 1
+def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner, model_use_lateral_planner):
   if model_use_lateral_planner:
     plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS)
     model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS)
@@ -47,7 +45,10 @@ def plannerd_thread():
   debug_mode = bool(int(os.getenv("DEBUG", "0")))
 
   longitudinal_planner = LongitudinalPlanner(CP)
-  lateral_planner = LateralPlanner(CP, debug=debug_mode)
+
+  custom_model, model_gen = get_model_generation(params)
+  model_use_lateral_planner = custom_model and model_gen == 1
+  lateral_planner = LateralPlanner(CP, debug=debug_mode, model_use_lateral_planner=model_use_lateral_planner)
   lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED']
 
   pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'longitudinalPlanSP'] + lateral_planner_svs)
@@ -64,7 +65,7 @@ def plannerd_thread():
       lateral_planner.publish(sm, pm)
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
-      publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
+      publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner, model_use_lateral_planner)
 
 def main():
   plannerd_thread()

selfdrive/modeld/fill_model_msg.py

@@ -4,7 +4,6 @@ import numpy as np
 from typing import Dict
 from cereal import log
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
-from openpilot.selfdrive.sunnypilot import get_model_generation
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
@@ -47,8 +46,7 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std
 def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState,
                    vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
                    timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
-                   nav_enabled: bool, v_ego: float, steer_delay: float, valid: bool) -> None:
-  custom_model, model_gen = get_model_generation()
+                   nav_enabled: bool, v_ego: float, steer_delay: float, valid: bool, model_use_lateral_planner: bool) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   msg.valid = valid
 
@@ -75,7 +73,7 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
   # lateral planning
-  if custom_model and model_gen == 1:
+  if model_use_lateral_planner:
     solution = modelV2.lateralPlannerSolutionDEPRECATED
     solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
     solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]

selfdrive/modeld/modeld.py

@@ -59,23 +59,24 @@ class ModelState:
   model: ModelRunner
 
   def __init__(self, context: CLContext):
-    self.custom_model, self.model_gen = get_model_generation()
+    self.param_s = Params()
+    self.custom_model, self.model_gen = get_model_generation(self.param_s)
     self.frame = ModelFrame(context)
     self.wide_frame = ModelFrame(context)
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
     _inputs = {
-      'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),  # gen2/3
-      'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),  # gen3
+      'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
+      'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
     }
     if self.custom_model:
       if self.model_gen == 1:
         _inputs = {
-          'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32),  # gen1
+          'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32),
         }
-      if self.model_gen == 2:  # gen2
+      if self.model_gen == 2:
         _inputs = {
-          'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),  # gen2/3
-          'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32),  # gen2
+          'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
+          'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32),
         }
 
     self.inputs = {
@@ -87,8 +88,6 @@ class ModelState:
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
     }
 
-    self.param_s = Params()
-
     if self.param_s.get_bool("CustomDrivingModel"):
       _model_name = self.param_s.get("DrivingModelText", encoding="utf8")
       _model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"}
@@ -167,8 +166,6 @@ def main(demo=False):
   model = ModelState(cl_context)
   cloudlog.warning("models loaded, modeld starting")
 
-  custom_model, model_gen = get_model_generation()
-
   # visionipc clients
   while True:
     available_streams = VisionIpcClient.available_streams("camerad", block=False)
@@ -199,6 +196,7 @@ def main(demo=False):
 
   publish_state = PublishState()
   params = Params()
+  custom_model, model_gen = get_model_generation(params)
   if not (custom_model and model_gen == 1):
     with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg:
       steer_delay = msg.steerActuatorDelay + .2
@@ -352,7 +350,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
-                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, v_ego, steer_delay, live_calib_seen)
+                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, v_ego, steer_delay, live_calib_seen, custom_model and model_gen == 1)
 
       if not (custom_model and model_gen == 1):
         desire_state = modelv2_send.modelV2.meta.desireState

selfdrive/sunnypilot/__init__.py

@@ -1,8 +1,7 @@
 from openpilot.common.params import Params
 
 
-def get_model_generation():
-  params = Params()
+def get_model_generation(params=Params()):
   custom_model = params.get_bool("CustomDrivingModel")
   gen = int(params.get("DrivingModelGeneration", encoding="utf8"))
   return custom_model, gen