diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fe3ed9196f..442b6aa8ca 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -371,6 +371,15 @@ struct CarControlSP @0xa5cd762cd951a455 { leadOne @2 :LeadData; leadTwo @3 :LeadData; intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement; + radarTracks @5 :List(RadarTrack); + radarTracksActive @6 :Bool; + + struct RadarTrack { + trackId @0 :UInt64; + dRel @1 :Float32; + yRel @2 :Float32; + vRel @3 :Float32; + } struct Param { key @0 :Text; diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 384152c71b..001e53cdec 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -60,6 +60,7 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct # 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', {}))) + struct_dataclass.radarTracks = [structs.CarControlSP.RadarTrack(**track) for track in struct_dict.get('radarTracks', [])] struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement( **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {})) ) diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index c8d054243f..5cec3bf855 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -31,7 +31,7 @@ class ControlsExt(ModelStateBase): self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP) cloudlog.info("controlsd_ext got CarParamsSP") - self.sm_services_ext = ['radarState', 'selfdriveStateSP'] + self.sm_services_ext = ['radarState', 'selfdriveStateSP', 'liveTracks'] self.pm_services_ext = ['carControlSP'] def initialize_lateral_control(self, lac, CI, dt): @@ -91,6 +91,15 @@ class ControlsExt(ModelStateBase): self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne) self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo) + live_tracks = sm['liveTracks'] + CC_SP.radarTracksActive = sm.valid['liveTracks'] and len(live_tracks.trackSources) > 0 + radar_tracks = CC_SP.init('radarTracks', len(live_tracks.points) if CC_SP.radarTracksActive else 0) + for dst, src in zip(radar_tracks, live_tracks.points, strict=True): + dst.trackId = src.trackId + dst.dRel = src.dRel + dst.yRel = src.yRel + dst.vRel = src.vRel + # MADS state mads_src = sm['selfdriveStateSP'].mads CC_SP.mads.state = mads_src.state