Try this for ModelDataV2.Action

selfdrive/controls/lib/longitudinal_planner.py

@@ -171,16 +171,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.generation == 11:
-      if self.mode == 'acc':
-        output_a_target = output_a_target_mpc
-        self.output_should_stop = output_should_stop_mpc
-      else:
-        output_a_target = min(output_a_target_mpc, output_a_target_e2e)
-        self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+    if self.mode == 'acc':
+      output_a_target = output_a_target_mpc
+      self.output_should_stop = output_should_stop_mpc
     else:
-      output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
-                                                                     action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
+      output_a_target = min(output_a_target_mpc, output_a_target_e2e)
+      self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+
+    if not (self.generation == 11):
+      output_a_target, self.output_should_stop = output_a_target_mpc, output_should_stop_mpc
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

sunnypilot/modeld/fill_model_msg.py

@@ -50,7 +50,7 @@ def fill_xyz_poly(builder, degree, x, y, z):
   builder.zCoefficients = coeffs[:, 2].tolist()
 
 def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
-                   net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState,
+                   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, model_execution_time: float, valid: bool,
                    v_ego: float, steer_delay: float, meta_const) -> None:
@@ -78,8 +78,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
 
   # Populate drivingModelData.action
   driving_model_data_action = driving_model_data.action
-  driving_model_data_action.desiredAcceleration = action.desiredAcceleration
-  driving_model_data_action.shouldStop = action.shouldStop
   driving_model_data_action.desiredCurvature = desired_curvature
 
   modelV2 = extended_msg.modelV2
@@ -115,8 +113,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
 
   # lateral planning
   modelV2_action = modelV2.action
-  modelV2_action.desiredAcceleration = action.desiredAcceleration
-  modelV2_action.shouldStop = action.shouldStop
   modelV2_action.desiredCurvature = desired_curvature
 
   # times at X_IDXS according to model plan

sunnypilot/modeld/modeld.py

@@ -18,12 +18,11 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.system import sentry
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
 
 from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime
 from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
 from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
-from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
+from openpilot.sunnypilot.modeld.constants import ModelConstants
 from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
 from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd
 from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
@@ -62,8 +61,7 @@ class ModelState:
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
     bundle = get_active_bundle()
     overrides = {override.key: override.value for override in bundle.overrides}
-    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2"))
-    self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
+    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
 
     model_paths = get_model_path()
     self.model_metadata = load_metadata()
@@ -127,14 +125,6 @@ class ModelState:
         self.inputs['prev_desired_curv'][-1:] = outputs['desired_curvature'][0, :]
     return outputs
 
-  def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                            long_action_t: float) -> log.ModelDataV2.Action:
-    plan = model_output['plan'][0]
-    desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], ModelConstants.T_IDXS,
-                                                     action_t=long_action_t)
-    desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
-
-    return log.ModelDataV2.Action(desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
 
 
 def main(demo=False):
@@ -204,10 +194,6 @@ def main(demo=False):
 
   modeld_lagd = ModeldLagd()
 
-  # Enable lagd support for sunnypilot modeld
-  long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
-  prev_action = log.ModelDataV2.Action()
-
   DH = DesireHelper()
 
   while True:
@@ -303,8 +289,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
-      action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL)
-      fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
+      fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
                      frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
                      v_ego, steer_delay, model.meta)
 

sunnypilot/modeld_v2/modeld.py

@@ -57,7 +57,7 @@ class ModelState:
     self.generation = model_bundle.generation
     overrides = {override.key: override.value for override in model_bundle.overrides}
 
-    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2"))
+    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
     self.MIN_LAT_CONTROL_SPEED = 0.3
 
@@ -172,7 +172,10 @@ class ModelState:
     else:
       desired_curvature = prev_action.desiredCurvature
 
-    return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
+    if self.generation == 11:
+      return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
+    else:
+      return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature))
 
 
 def main(demo=False):