carControlSP: parse radarState's leadOne & leadTwo (#1089)

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

* typo

* bump

* convert first

* rename

* directly

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;

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

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