From 5b137d666f6b4ce343443c92b4c5d6b6465cafba Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 29 Jul 2025 10:53:06 -0400
Subject: [PATCH] `carControlSP`: parse `radarState`'s `leadOne` & `leadTwo`
 (#1082)

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

* typo

* bump
---
 cereal/custom.capnp                           | 23 +++++++++++++++++++
 opendbc_repo                                  |  2 +-
 selfdrive/car/helpers.py                      |  2 ++
 .../selfdrive/controls/controlsd_ext.py       |  6 ++++-
 4 files changed, 31 insertions(+), 2 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..fc89091ad 100644
--- a/sunnypilot/selfdrive/controls/controlsd_ext.py
+++ b/sunnypilot/selfdrive/controls/controlsd_ext.py
@@ -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:
@@ -47,6 +47,10 @@ class ControlsExt:
   def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
     CC_SP = custom.CarControlSP.new_message()
 
+    if sm.updated['radarState']:
+      CC_SP.leadOne = sm['radarState'].leadOne
+      CC_SP.leadTwo = sm['radarState'].leadTwo
+
     # MADS state
     CC_SP.mads = sm['selfdriveStateSP'].mads