This commit is contained in:
royjr
2026-07-16 02:19:45 -04:00
parent ab073dcbb4
commit 13e2da2e5a
4 changed files with 1 additions and 58 deletions
-9
View File
@@ -371,15 +371,6 @@ 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;
-1
View File
@@ -60,7 +60,6 @@ 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', {}))
)
+1 -14
View File
@@ -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', 'liveTracks']
self.sm_services_ext = ['radarState', 'selfdriveStateSP']
self.pm_services_ext = ['carControlSP']
def initialize_lateral_control(self, lac, CI, dt):
@@ -85,25 +85,12 @@ class ControlsExt(ModelStateBase):
_lead.radar = src.radar
_lead.radarTrackId = src.radarTrackId
@staticmethod
def get_radar_track_data(CC_SP: custom.CarControlSP, live_tracks, valid: bool) -> None:
CC_SP.radarTracksActive = valid and len(live_tracks.trackSources) > 0
source_tracks = live_tracks.points if CC_SP.radarTracksActive else ()
radar_tracks = CC_SP.init('radarTracks', len(source_tracks))
for dst, src in zip(radar_tracks, source_tracks, strict=True):
dst.trackId = src.trackId
dst.dRel = src.dRel
dst.yRel = src.yRel
dst.vRel = src.vRel
def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
CC_SP = custom.CarControlSP.new_message()
self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne)
self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo)
self.get_radar_track_data(CC_SP, sm['liveTracks'], sm.valid['liveTracks'])
# MADS state
mads_src = sm['selfdriveStateSP'].mads
CC_SP.mads.state = mads_src.state
@@ -1,34 +0,0 @@
from cereal import car, custom
from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
def make_live_tracks(with_source: bool):
live_tracks = car.RadarData.new_message()
point = live_tracks.init("points", 1)[0]
point.trackId = 7
point.dRel = 12
point.yRel = -3
point.vRel = 1
if with_source:
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 1}]
return live_tracks
def test_inactive_radar_tracks_ignores_stale_points():
cc_sp = custom.CarControlSP.new_message()
ControlsExt.get_radar_track_data(cc_sp, make_live_tracks(with_source=False), valid=True)
assert not cc_sp.radarTracksActive
assert len(cc_sp.radarTracks) == 0
def test_active_radar_tracks_are_copied():
cc_sp = custom.CarControlSP.new_message()
ControlsExt.get_radar_track_data(cc_sp, make_live_tracks(with_source=True), valid=True)
assert cc_sp.radarTracksActive
assert len(cc_sp.radarTracks) == 1
assert cc_sp.radarTracks[0].trackId == 7