Longitudinal planner: expose custom vTarget and aTarget (#1267)

cereal/custom.capnp

@@ -146,6 +146,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
   speedLimit @3 :SpeedLimit;
+  vTarget @4 :Float32;
+  aTarget @5 :Float32;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -24,6 +24,9 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = Source.cruise
 
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+
   @property
   def mlsim(self) -> bool:
     # If we don't have a generation set, we assume it's default model. Which as of today are mlsim.
@@ -47,9 +50,8 @@ class LongitudinalPlannerSP:
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
-    v_target, a_target = targets[self.source]
-
-    return v_target, a_target
+    self.output_v_target, self.output_a_target = targets[self.source]
+    return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
@@ -61,6 +63,8 @@ class LongitudinalPlannerSP:
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
     longitudinalPlanSP.longitudinalPlanSource = self.source
+    longitudinalPlanSP.vTarget = float(self.output_v_target)
+    longitudinalPlanSP.aTarget = float(self.output_a_target)
 
     # Dynamic Experimental Control
     dec = longitudinalPlanSP.dec