From 5f3821c1f9ce589d4bd37ae7fbc5ce6ad94fa825 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sat, 20 Sep 2025 12:21:13 -0400
Subject: [PATCH] Longitudinal planner: expose custom vTarget and aTarget
 (#1267)

---
 cereal/custom.capnp                                    |  2 ++
 .../selfdrive/controls/lib/longitudinal_planner.py     | 10 +++++++---
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/cereal/custom.capnp b/cereal/custom.capnp
index 652783809..c4360ae31 100644
--- a/cereal/custom.capnp
+++ b/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;
diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
index b4f4fe6d7..3e4dbd9ab 100644
--- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/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