From 713f8bfc21174f98e04d888ae18de0f71aaa2830 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 31 Jul 2025 12:56:33 -0400
Subject: [PATCH] `carControlSP`: parse `radarState`'s `leadOne` & `leadTwo`
 (#1089)

* `carControlSP`: parse `radarState`'s `leadOne` & `leadTwo`

* typo

* bump

* convert first

* rename

* directly
---
 cereal/custom.capnp                           | 23 ++++++++++++++++
 opendbc_repo                                  |  2 +-
 selfdrive/car/helpers.py                      |  2 ++
 .../selfdrive/controls/controlsd_ext.py       | 27 +++++++++++++++++--
 4 files changed, 51 insertions(+), 3 deletions(-)

diff --git a/cereal/custom.capnp b/cereal/custom.capnp
index c9e4f3e16..fdb89da84 100644
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -25,6 +25,27 @@ struct ModularAssistiveDrivingSystem {
   }
 }
 
+# Same struct as Log.RadarState.LeadData
+struct LeadData {
+  dRel @0 :Float32;
+  yRel @1 :Float32;
+  vRel @2 :Float32;
+  aRel @3 :Float32;
+  vLead @4 :Float32;
+  dPath @6 :Float32;
+  vLat @7 :Float32;
+  vLeadK @8 :Float32;
+  aLeadK @9 :Float32;
+  fcw @10 :Bool;
+  status @11 :Bool;
+  aLeadTau @12 :Float32;
+  modelProb @13 :Float32;
+  radar @14 :Bool;
+  radarTrackId @15 :Int32 = -1;
+
+  aLeadDEPRECATED @5 :Float32;
+}
+
 struct SelfdriveStateSP @0x81c2f05a394cf4af {
   mads @0 :ModularAssistiveDrivingSystem;
 }
@@ -174,6 +195,8 @@ struct CarParamsSP @0x80ae746ee2596b11 {
 struct CarControlSP @0xa5cd762cd951a455 {
   mads @0 :ModularAssistiveDrivingSystem;
   params @1 :List(Param);
+  leadOne @2 :LeadData;
+  leadTwo @3 :LeadData;
 
   struct Param {
     key @0 :Text;
diff --git a/opendbc_repo b/opendbc_repo
index b68fab9ea..25a5079a6 160000
--- a/opendbc_repo
+++ b/opendbc_repo
@@ -1 +1 @@
-Subproject commit b68fab9ea4052213f8bfc1b80832ac8d4e798c8b
+Subproject commit 25a5079a6ed31a3de2b4c2444720dbee9d162c52
diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py
index 6187be147..275c84479 100644
--- a/selfdrive/car/helpers.py
+++ b/selfdrive/car/helpers.py
@@ -58,5 +58,7 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
 
   struct_dataclass.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {})))
   struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])]
+  struct_dataclass.leadOne = structs.LeadData(**remove_deprecated(struct_dict.get('leadOne', {})))
+  struct_dataclass.leadTwo = structs.LeadData(**remove_deprecated(struct_dict.get('leadTwo', {})))
 
   return struct_dataclass
diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py
index 9a7f0cb5f..7e06ac77c 100644
--- a/sunnypilot/selfdrive/controls/controlsd_ext.py
+++ b/sunnypilot/selfdrive/controls/controlsd_ext.py
@@ -5,7 +5,7 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import cereal.messaging as messaging
-from cereal import custom
+from cereal import log, custom
 
 from opendbc.car import structs
 from openpilot.common.params import Params
@@ -26,7 +26,7 @@ class ControlsExt:
     self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
     cloudlog.info("controlsd_ext got CarParamsSP")
 
-    self.sm_services_ext = ['selfdriveStateSP']
+    self.sm_services_ext = ['radarState', 'selfdriveStateSP']
     self.pm_services_ext = ['carControlSP']
 
   def get_params_sp(self) -> None:
@@ -44,9 +44,32 @@ class ControlsExt:
     # MADS not available, use stock state to engage
     return bool(sm['selfdriveState'].active)
 
+  @staticmethod
+  def get_lead_data(ld: log.RadarState.LeadData) -> dict:
+    return {
+      "dRel": ld.dRel,
+      "yRel": ld.yRel,
+      "vRel": ld.vRel,
+      "aRel": ld.aRel,
+      "vLead": ld.vLead,
+      "dPath": ld.dPath,
+      "vLat": ld.vLat,
+      "vLeadK": ld.vLeadK,
+      "aLeadK": ld.aLeadK,
+      "fcw": ld.fcw,
+      "status": ld.status,
+      "aLeadTau": ld.aLeadTau,
+      "modelProb": ld.modelProb,
+      "radar": ld.radar,
+      "radarTrackId": ld.radarTrackId,
+    }
+
   def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
     CC_SP = custom.CarControlSP.new_message()
 
+    CC_SP.leadOne = self.get_lead_data(sm['radarState'].leadOne)
+    CC_SP.leadTwo = self.get_lead_data(sm['radarState'].leadTwo)
+
     # MADS state
     CC_SP.mads = sm['selfdriveStateSP'].mads