mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-23 10:02:05 +08:00
Compare commits
322 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| ca15ab474c | |||
| c6d62ffdaf | |||
| a4fcf02b36 | |||
| f5ca90a065 | |||
| 66a664ed42 | |||
| c569b11472 | |||
| 3d3e6e37b2 | |||
| c33e7c1294 | |||
| 3e38f8d6fe | |||
| 9a510fd556 | |||
| bd062ec236 | |||
| 17784f72c1 | |||
| 5a1855b69a | |||
| ccefe172bd | |||
| 037a602a14 | |||
| 1b5b4be457 | |||
| 892efd4fd2 | |||
| a9063dc09e | |||
| 1a9546247a | |||
| 287015e4fb | |||
| 575c4b03cf | |||
| 30a607faee | |||
| aeacb8387c | |||
| b7ac1a1bf6 | |||
| 80472a73e3 | |||
| 78c265a653 | |||
| bdf9b955d1 | |||
| 629c31e71f | |||
| 13e2da2e5a | |||
| ab073dcbb4 | |||
| aeb9b412fe | |||
| b0322f5722 | |||
| aa636e75c8 | |||
| 5ff3c3bd8d | |||
| 2e9e07b2e0 | |||
| e1a87d7f5b | |||
| c177a15b60 | |||
| a159575710 | |||
| 3ae153d464 | |||
| e4057bd700 | |||
| c796e0df0a | |||
| 3c58d5f527 | |||
| efb028a50f | |||
| a09d9fba76 | |||
| 9de5782527 | |||
| 13d0810727 | |||
| 20c543b9ba | |||
| a11d11c0d3 | |||
| 6ad21b7da8 | |||
| 4d0caff9d9 | |||
| e3e59eeebf | |||
| cf292b99b8 | |||
| 1ec96f97a7 | |||
| 1252095f45 | |||
| 6d204f173e | |||
| a4de865768 | |||
| cf0c6e7d4b | |||
| adf6c0f3d4 | |||
| 44a865813b | |||
| 28bdc052e0 | |||
| fcdfeb1bec | |||
| 6ad7a58d12 | |||
| c8404ffa39 | |||
| 6ee7563c53 | |||
| f69666c774 | |||
| 8485176a0f | |||
| f33d861e85 | |||
| c58e18794f | |||
| 23bda78e7a | |||
| 768cd4a0ad | |||
| dd90c9fa49 | |||
| 6999e13130 | |||
| 1282c32869 | |||
| 65ed51d908 | |||
| 0a222d406f | |||
| 572ebe35ec | |||
| 9a7d28c672 | |||
| 27d348d364 | |||
| f42d5b8c16 | |||
| 20f98519dd | |||
| 57d75160a5 | |||
| 5435a5b97c | |||
| 002e6d11f3 | |||
| 1e11b52290 | |||
| 76e0204025 | |||
| b1b658c506 | |||
| cc88f2cbd6 | |||
| be8abfe8f8 | |||
| 453e1d9514 | |||
| 98a40ed15c | |||
| 267db8b817 | |||
| ee0ac199a4 | |||
| b9ce161fff | |||
| d5ed828eaa | |||
| 9fca585f2a | |||
| bb1259303e | |||
| 8562e39b0b | |||
| e135051ca8 | |||
| 12bef55d8a | |||
| be4223fe13 | |||
| 2b492098e9 | |||
| 00fee3ee88 | |||
| 930ab1a84d | |||
| 0c77f1b41b | |||
| 9fd4e5b9f8 | |||
| 818dd7c4e3 | |||
| 2f7a45e6c8 | |||
| 936ebfc12b | |||
| aa0c9dc0eb | |||
| 7476a866e7 | |||
| 610d857e33 | |||
| d2f47407d0 | |||
| db75ec76ea | |||
| 24066465d7 | |||
| a7abbd6e25 | |||
| 878982447c | |||
| 576527a36b | |||
| ef8c35da24 | |||
| 85688b1040 | |||
| 0fb2199130 | |||
| d48d756c1d | |||
| 2ed298a0c9 | |||
| d68f038949 | |||
| 7231571e57 | |||
| b37f1419d3 | |||
| 9fe6d20249 | |||
| e31b28d0b3 | |||
| cd85a66790 | |||
| 305ea87daf | |||
| 4bbfc793e0 | |||
| d5d983676e | |||
| de8a96a398 | |||
| 0cbf45f699 | |||
| 06498600bf | |||
| 0d68a3a2ab | |||
| 9e85a85059 | |||
| 0373c327c0 | |||
| efe9e5c200 | |||
| 8a249a45dc | |||
| bdbefe67f6 | |||
| 05649045b7 | |||
| 0ee21708ac | |||
| 1492941483 | |||
| 675bb166ad | |||
| 1b717a7e88 | |||
| 86f55a8ba9 | |||
| 8794178ccf | |||
| 86303e7a3c | |||
| 629392d2f7 | |||
| bc414bdc8b | |||
| 7ca5649f2c | |||
| 641ee8fa87 | |||
| e189678033 | |||
| 99bae4f475 | |||
| 56c276158c | |||
| c65308a8bd | |||
| 994e526460 | |||
| 1defae36b7 | |||
| 8f029fd0ef | |||
| ddb46284dc | |||
| ef7046c2ed | |||
| 734121b517 | |||
| 9effc754d9 | |||
| e49ffc2a2d | |||
| b738769c22 | |||
| 0bfd9e5cf2 | |||
| 20fadbcaa8 | |||
| 2cacd0b3e5 | |||
| c4b8859dff | |||
| 8fb0953205 | |||
| 63d1c8835f | |||
| 17a185606d | |||
| 0eeb052241 | |||
| edc2d16ae4 | |||
| e72fa9453d | |||
| 1c56f0b0a3 | |||
| da10131392 | |||
| 7107c2ba14 | |||
| 95b6e877ac | |||
| eb02c6570e | |||
| 80d15212e2 | |||
| ab2863a859 | |||
| 1be8ae31c4 | |||
| 04dcd38856 | |||
| 22ccf0d72f | |||
| 3c969bb627 | |||
| 20f8011feb | |||
| 9cf17e74a1 | |||
| 2c4efdf557 | |||
| 4cd3d3c16c | |||
| 637f3ae9c8 | |||
| 464ee80f71 | |||
| 2743a04613 | |||
| 7f9978d001 | |||
| 4b83961c67 | |||
| c00eaf428a | |||
| 0a9993e8d4 | |||
| 0af214a985 | |||
| af43385e3a | |||
| 0ab2b8c590 | |||
| 67ab18a0de | |||
| e87dc15b30 | |||
| 192d08516c | |||
| 3cf001c59c | |||
| f2ccd021da | |||
| c9fc900f64 | |||
| 3c37c5ce5d | |||
| 7c45889e4e | |||
| 2aabb7aee8 | |||
| 3859e9962f | |||
| 810efbab72 | |||
| ec27bec326 | |||
| 250d553157 | |||
| cea54a0ca8 | |||
| 8e72d783bd | |||
| 1b0dc103dc | |||
| 6c364d292b | |||
| bcdec2ce84 | |||
| 3deaeb3759 | |||
| c669f0984a | |||
| 743aa8e736 | |||
| 5847256371 | |||
| c13f909390 | |||
| f1de835d17 | |||
| 76ddb20cd5 | |||
| ede7f70ddc | |||
| 46c3047fa7 | |||
| 46dd946740 | |||
| 9da4b3653e | |||
| 4e21ae7c50 | |||
| bb91e92237 | |||
| 14b4c4f85b | |||
| 0660b542c3 | |||
| 2b893b90c9 | |||
| f5139178ed | |||
| fb43b755f2 | |||
| cf5b5c666d | |||
| 03b56d6f09 | |||
| 07f5b967d8 | |||
| ea19c7d3bb | |||
| e461842cbb | |||
| a73c9659d5 | |||
| ab180ce1e5 | |||
| e0d8bc88b2 | |||
| 1dc45adb1c | |||
| c3bd613885 | |||
| 55edb4efcb | |||
| cb796fbc76 | |||
| 6bf75fc557 | |||
| 9a1fc28819 | |||
| 0741d05e92 | |||
| 1ad008107d | |||
| feebd9df93 | |||
| c2e5ced3e5 | |||
| 15e5d2efb9 | |||
| a3929d0b54 | |||
| 794f8f9991 | |||
| 68fa5e3f21 | |||
| 86c6cc1f48 | |||
| eb7ffbf093 | |||
| 3919095752 | |||
| 74d63be1c3 | |||
| 8894486a1a | |||
| 810599315d | |||
| 6f3ab810c8 | |||
| 230f78b8d3 | |||
| f1affec088 | |||
| 7ff9b28c94 | |||
| 5f9d340d7b | |||
| 97d8ef242c | |||
| a63fff9b45 | |||
| cb3893daaa | |||
| e82b47cb70 | |||
| 10b5e58558 | |||
| 116936341f | |||
| 29f60df74b | |||
| c6c072e1f4 | |||
| 8d5bd92d51 | |||
| 4453dba6ed | |||
| cc20313815 | |||
| 3b30b93bbb | |||
| 5d9de36d40 | |||
| d101cbb83e | |||
| 1536d59633 | |||
| dc99b865ae | |||
| e59bc027ff | |||
| cf7e5efaca | |||
| 4b44f2eb31 | |||
| 107d2ab400 | |||
| 5432d9062c | |||
| f533f6c843 | |||
| 58e9ac763c | |||
| cb50d54169 | |||
| bd5de4ed0a | |||
| 0d4073fadb | |||
| ebc70dcb52 | |||
| 4d0426999e | |||
| 286da42573 | |||
| 8a836710a9 | |||
| 5d515bcf33 | |||
| 1c7f6d5133 | |||
| 05d57c7aeb | |||
| a91b97b89a | |||
| ec4e2ec3c1 | |||
| 1f49367380 | |||
| 040f81fc91 | |||
| e4b0eaf352 | |||
| a710276472 | |||
| af086db671 | |||
| 0d9eb0e25e | |||
| 0616caed6d | |||
| 095337b3c1 | |||
| 1edec2d22c | |||
| affabb9ee0 | |||
| dc27e8711c | |||
| cf7329a264 | |||
| 5ee5ecd820 | |||
| b064f730dd | |||
| 5253b33b7a | |||
| fc46a0ed7f | |||
| 8056a797c7 | |||
| a492625927 |
+1
-1
Submodule opendbc_repo updated: 0ed33dae96...e5e07d796d
@@ -371,6 +371,17 @@ 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;
|
||||
motionState @4 :UInt8;
|
||||
age @5 :UInt16;
|
||||
}
|
||||
|
||||
struct Param {
|
||||
key @0 :Text;
|
||||
|
||||
@@ -177,6 +177,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RadarTracks", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
@@ -182,6 +182,8 @@ class Car:
|
||||
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
self.radar_tracks = int(self.params.get("RadarTracks", return_default=True))
|
||||
self._applied_radar_tracks = getattr(self.RI, "radar_mode", self.radar_tracks)
|
||||
|
||||
# card is driven by can recv, expected at 100Hz
|
||||
self.rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
@@ -199,6 +201,8 @@ class Car:
|
||||
CS, CS_SP = self.CI.update(can_list)
|
||||
CS_SP = convert_to_capnp(CS_SP)
|
||||
|
||||
self._update_radar_tracks()
|
||||
|
||||
# Update radar tracks from CAN
|
||||
RD: structs.RadarDataT | None = self.RI.update(can_list)
|
||||
|
||||
@@ -225,6 +229,18 @@ class Car:
|
||||
|
||||
return CS, CS_SP, RD
|
||||
|
||||
def _update_radar_tracks(self) -> None:
|
||||
if self.CP.brand != "hyundai" or self.radar_tracks == self._applied_radar_tracks:
|
||||
return
|
||||
|
||||
if self.RI.set_radar_mode(self.radar_tracks):
|
||||
self.CP_SP_capnp = convert_to_capnp(self.CP_SP)
|
||||
cp_sp_bytes = self.CP_SP_capnp.to_bytes()
|
||||
self.params.put("CarParamsSP", cp_sp_bytes)
|
||||
self.params.put("CarParamsSPCache", cp_sp_bytes)
|
||||
self.params.put("CarParamsSPPersistent", cp_sp_bytes)
|
||||
self._applied_radar_tracks = self.radar_tracks
|
||||
|
||||
def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None):
|
||||
"""carState and carParams publish loop"""
|
||||
|
||||
@@ -303,6 +319,7 @@ class Car:
|
||||
while not evt.is_set():
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
|
||||
self.radar_tracks = int(self.params.get("RadarTracks", return_default=True))
|
||||
|
||||
# sunnypilot
|
||||
self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
|
||||
|
||||
@@ -49,19 +49,61 @@ def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP) -> capnp.
|
||||
|
||||
|
||||
def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> structs.CarControlSP:
|
||||
# TODO: recursively handle any car struct as needed
|
||||
def remove_deprecated(s: dict) -> dict:
|
||||
return {k: v for k, v in s.items() if not k.endswith('DEPRECATED')}
|
||||
def convert_lead(src) -> structs.LeadData:
|
||||
lead_is_default = not any((
|
||||
src.dRel, src.yRel, src.vRel, src.aRel, src.vLead, src.aLeadDEPRECATED, src.dPath, src.vLat,
|
||||
src.vLeadK, src.aLeadK, src.fcw, src.status, src.aLeadTau, src.modelProb, src.radar,
|
||||
src.radarTrackId != -1,
|
||||
))
|
||||
if lead_is_default:
|
||||
return structs.LeadData()
|
||||
|
||||
struct_dict = struct.to_dict()
|
||||
struct_dataclass = structs.CarControlSP(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)}))
|
||||
return structs.LeadData(
|
||||
dRel=src.dRel,
|
||||
yRel=src.yRel,
|
||||
vRel=src.vRel,
|
||||
aRel=src.aRel,
|
||||
vLead=src.vLead,
|
||||
dPath=src.dPath,
|
||||
vLat=src.vLat,
|
||||
vLeadK=src.vLeadK,
|
||||
aLeadK=src.aLeadK,
|
||||
fcw=src.fcw,
|
||||
status=src.status,
|
||||
aLeadTau=src.aLeadTau,
|
||||
modelProb=src.modelProb,
|
||||
radar=src.radar,
|
||||
radarTrackId=src.radarTrackId,
|
||||
)
|
||||
|
||||
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', {})))
|
||||
struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
|
||||
**remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
|
||||
return structs.CarControlSP(
|
||||
mads=structs.ModularAssistiveDrivingSystem(
|
||||
state=str(struct.mads.state),
|
||||
enabled=struct.mads.enabled,
|
||||
active=struct.mads.active,
|
||||
available=struct.mads.available,
|
||||
),
|
||||
params=[
|
||||
{"key": param.key, "type": str(param.type), "value": bytes(param.value)}
|
||||
for param in struct.params
|
||||
],
|
||||
leadOne=convert_lead(struct.leadOne),
|
||||
leadTwo=convert_lead(struct.leadTwo),
|
||||
intelligentCruiseButtonManagement=structs.IntelligentCruiseButtonManagement(
|
||||
state=str(struct.intelligentCruiseButtonManagement.state),
|
||||
sendButton=str(struct.intelligentCruiseButtonManagement.sendButton),
|
||||
vTarget=struct.intelligentCruiseButtonManagement.vTarget,
|
||||
),
|
||||
radarTracks=[
|
||||
structs.CarControlSP.RadarTrack(
|
||||
trackId=track.trackId,
|
||||
dRel=track.dRel,
|
||||
yRel=track.yRel,
|
||||
vRel=track.vRel,
|
||||
motionState=track.motionState,
|
||||
age=track.age,
|
||||
)
|
||||
for track in struct.radarTracks
|
||||
],
|
||||
radarTracksActive=struct.radarTracksActive,
|
||||
)
|
||||
|
||||
return struct_dataclass
|
||||
|
||||
@@ -5,7 +5,7 @@ from openpilot.common.parameterized import parameterized
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car import DT_CTRL
|
||||
from opendbc.car import DT_CTRL, structs
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface
|
||||
from opendbc.car.mock.values import CAR as MOCK
|
||||
@@ -22,6 +22,45 @@ from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfac
|
||||
MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '60'))
|
||||
|
||||
|
||||
def legacy_convert_car_control_sp(struct):
|
||||
def remove_deprecated(values):
|
||||
return {k: v for k, v in values.items() if not k.endswith('DEPRECATED')}
|
||||
|
||||
struct_dict = struct.to_dict()
|
||||
result = structs.CarControlSP(**remove_deprecated(struct_dict))
|
||||
result.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {})))
|
||||
result.leadOne = structs.LeadData(**remove_deprecated(struct_dict.get('leadOne', {})))
|
||||
result.leadTwo = structs.LeadData(**remove_deprecated(struct_dict.get('leadTwo', {})))
|
||||
result.radarTracks = [structs.CarControlSP.RadarTrack(**track) for track in struct_dict.get('radarTracks', [])]
|
||||
result.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
|
||||
**remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def test_convert_car_control_sp_matches_legacy_conversion():
|
||||
msg = custom.CarControlSP.new_message(
|
||||
mads={"state": "enabled", "enabled": True, "active": True, "available": True},
|
||||
params=[{"key": "test", "type": "int", "value": b"42"}],
|
||||
leadOne={
|
||||
"dRel": 12.5, "yRel": -0.25, "vRel": -1.5, "aRel": 0.1, "vLead": 20.0,
|
||||
"dPath": 0.5, "vLat": 0.2, "vLeadK": 20.1, "aLeadK": 0.05, "fcw": True,
|
||||
"status": True, "aLeadTau": 1.5, "modelProb": 0.9, "radar": True, "radarTrackId": 7,
|
||||
},
|
||||
leadTwo={"dRel": 25.0, "status": True, "radarTrackId": 9},
|
||||
intelligentCruiseButtonManagement={"state": "holding", "sendButton": "increase", "vTarget": 27.0},
|
||||
radarTracks=[
|
||||
{"trackId": 7, "dRel": 12.5, "yRel": -0.25, "vRel": -1.5, "motionState": 2, "age": 4},
|
||||
{"trackId": 9, "dRel": 25.0, "yRel": 3.0, "vRel": 0.5, "motionState": 1, "age": 8},
|
||||
],
|
||||
radarTracksActive=True,
|
||||
).as_reader()
|
||||
|
||||
assert convert_carControlSP(msg) == legacy_convert_car_control_sp(msg)
|
||||
default_msg = custom.CarControlSP.new_message().as_reader()
|
||||
assert convert_carControlSP(default_msg) == legacy_convert_car_control_sp(default_msg)
|
||||
|
||||
|
||||
class TestCarInterfaces:
|
||||
# FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause
|
||||
# many generated examples to overrun when max_examples > ~20, don't use it
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Priority, config_realtime_process
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
@@ -11,7 +12,7 @@ import openpilot.cereal.messaging as messaging
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process(5, Priority.CTRL_LOW)
|
||||
config_realtime_process(6 if HARDWARE.get_device_type() == "mici" else 5, Priority.CTRL_LOW)
|
||||
|
||||
cloudlog.info("plannerd is waiting for CarParams")
|
||||
params = Params()
|
||||
|
||||
@@ -26,6 +26,8 @@ SPEED, ACCEL = 0, 1 # Kalman filter states enum
|
||||
|
||||
# stationary qualification parameters
|
||||
V_EGO_STATIONARY = 4. # no stationary object flag below this speed
|
||||
DBC_MOTION_STATIONARY = 1
|
||||
DBC_MOTION_MOVING = 2
|
||||
|
||||
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
|
||||
|
||||
@@ -191,8 +193,14 @@ def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict
|
||||
return lead_dict
|
||||
|
||||
|
||||
def radar_point_eligible_for_fusion(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
point: car.RadarData.RadarPoint) -> bool:
|
||||
use_dbc_motion = CP.brand == "hyundai" and CP_SP.flags & HyundaiFlagsSP.RADAR_FULL_RADAR
|
||||
return not use_dbc_motion or point.motionState in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING)
|
||||
|
||||
|
||||
class RadarD:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float = 0.0):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, delay: float = 0.0):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
@@ -220,7 +228,11 @@ class RadarD:
|
||||
self.v_ego_hist.append(self.v_ego)
|
||||
self.last_v_ego_frame = sm.recv_frame['carState']
|
||||
|
||||
ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel] for pt in rr.points}
|
||||
ar_pts = {
|
||||
pt.trackId: [pt.dRel, pt.yRel, pt.vRel]
|
||||
for pt in rr.points
|
||||
if radar_point_eligible_for_fusion(self.CP, self.CP_SP, pt)
|
||||
}
|
||||
|
||||
# *** remove missing points from meta data ***
|
||||
for ids in list(self.tracks.keys()):
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.controls.radard import radar_point_eligible_for_fusion
|
||||
|
||||
|
||||
def radar_point(motion_state: int):
|
||||
point = car.RadarData.RadarPoint.new_message()
|
||||
point.motionState = motion_state
|
||||
return point
|
||||
|
||||
|
||||
def test_hyundai_full_radar_fuses_only_classified_points():
|
||||
CP = structs.CarParams(brand="hyundai")
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
CP_SP.flags = HyundaiFlagsSP.RADAR_FULL_RADAR.value
|
||||
|
||||
assert radar_point_eligible_for_fusion(CP, CP_SP, radar_point(1))
|
||||
assert radar_point_eligible_for_fusion(CP, CP_SP, radar_point(2))
|
||||
assert not radar_point_eligible_for_fusion(CP, CP_SP, radar_point(0))
|
||||
assert not radar_point_eligible_for_fusion(CP, CP_SP, radar_point(255))
|
||||
|
||||
|
||||
def test_radar_motion_filter_does_not_affect_other_modes_or_brands():
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
|
||||
assert radar_point_eligible_for_fusion(structs.CarParams(brand="hyundai"), CP_SP, radar_point(0))
|
||||
assert radar_point_eligible_for_fusion(structs.CarParams(brand="toyota"), CP_SP, radar_point(0))
|
||||
@@ -645,7 +645,7 @@ class SelfdriveD(CruiseHelper):
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process(4, Priority.CTRL_HIGH)
|
||||
config_realtime_process(6 if HARDWARE.get_device_type() == "mici" else 4, Priority.CTRL_HIGH)
|
||||
s = SelfdriveD()
|
||||
s.run()
|
||||
|
||||
|
||||
@@ -34,7 +34,11 @@ class MainLayout(Widget):
|
||||
# Initialize layouts
|
||||
self._home_layout = HomeLayout()
|
||||
self._home_body_layout = BodyLayout()
|
||||
self._layouts = {MainState.HOME: self._home_layout, MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()}
|
||||
self._layouts = {
|
||||
MainState.HOME: self._home_layout,
|
||||
MainState.SETTINGS: SettingsLayout(),
|
||||
MainState.ONROAD: AugmentedRoadView(radar_tracks_settings_callback=lambda: self.open_settings(PanelType.TOGGLES)),
|
||||
}
|
||||
|
||||
self._sidebar_rect = rl.Rectangle(0, 0, 0, 0)
|
||||
self._content_rect = rl.Rectangle(0, 0, 0, 0)
|
||||
|
||||
@@ -106,7 +106,18 @@ class TogglesLayout(Widget):
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._radar_tracks_setting = None
|
||||
self._toggles = {}
|
||||
if gui_app.sunnypilot_ui():
|
||||
self._radar_tracks_setting = multiple_button_item(
|
||||
lambda: tr("Radar Tracks"),
|
||||
"",
|
||||
buttons=[lambda: tr("Off"), lambda: tr("Lead Only"), lambda: tr("Full Radar")],
|
||||
button_width=250,
|
||||
callback=self._set_radar_tracks,
|
||||
selected_index=self._params.get("RadarTracks", return_default=True),
|
||||
)
|
||||
self._toggles["RadarTracks"] = self._radar_tracks_setting
|
||||
self._locked_toggles = set()
|
||||
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
|
||||
toggle = toggle_item(
|
||||
@@ -203,6 +214,8 @@ class TogglesLayout(Widget):
|
||||
# refresh toggles from params to mirror external changes
|
||||
for param in self._toggle_defs:
|
||||
self._toggles[param].action_item.set_state(self._params.get_bool(param))
|
||||
if self._radar_tracks_setting is not None:
|
||||
self._radar_tracks_setting.action_item.set_selected_button(self._params.get("RadarTracks", return_default=True))
|
||||
|
||||
# these toggles need restart, block while engaged
|
||||
for toggle_def in self._toggle_defs:
|
||||
@@ -247,3 +260,6 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _set_longitudinal_personality(self, button_index: int):
|
||||
self._params.put("LongitudinalPersonality", button_index, block=True)
|
||||
|
||||
def _set_radar_tracks(self, button_index: int):
|
||||
self._params.put("RadarTracks", button_index, block=True)
|
||||
|
||||
@@ -32,7 +32,8 @@ class MiciMainLayout(Scroller):
|
||||
self._home_layout = MiciHomeLayout()
|
||||
self._alerts_layout = MiciOffroadAlerts()
|
||||
self._settings_layout = SettingsLayout()
|
||||
self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
|
||||
self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked,
|
||||
radar_tracks_settings_callback=self._settings_layout.open_toggles)
|
||||
self._body_onroad_layout = BodyLayout()
|
||||
|
||||
# Initialize widget rects
|
||||
|
||||
@@ -20,9 +20,9 @@ class SettingsLayout(NavScroller):
|
||||
super().__init__()
|
||||
self._params = Params()
|
||||
|
||||
toggles_panel = TogglesLayoutMici()
|
||||
self._toggles_panel = TogglesLayoutMici()
|
||||
toggles_btn = SettingsBigButton("toggles", "", gui_app.texture("icons_mici/settings.png", 64, 64))
|
||||
toggles_btn.set_click_callback(lambda: gui_app.push_widget(toggles_panel))
|
||||
toggles_btn.set_click_callback(lambda: gui_app.push_widget(self._toggles_panel))
|
||||
|
||||
network_panel = NetworkLayoutMici()
|
||||
network_btn = SettingsBigButton("network", "", gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 76, 56))
|
||||
@@ -56,3 +56,8 @@ class SettingsLayout(NavScroller):
|
||||
])
|
||||
|
||||
self._font_medium = gui_app.font(FontWeight.MEDIUM)
|
||||
|
||||
def open_toggles(self) -> None:
|
||||
if not gui_app.widget_in_stack(self):
|
||||
gui_app.push_widget(self)
|
||||
gui_app.push_widget(self._toggles_panel)
|
||||
|
||||
@@ -21,8 +21,10 @@ class TogglesLayoutMici(NavScroller):
|
||||
record_front = BigParamControl("record & upload driver camera", "RecordFront", toggle_callback=restart_needed_callback)
|
||||
record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
|
||||
enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
|
||||
radar_tracks = BigMultiParamToggle("radar tracks", "RadarTracks", ["off", "lead only", "full radar"])
|
||||
|
||||
self._scroller.add_widgets([
|
||||
radar_tracks,
|
||||
self._personality_toggle,
|
||||
self._experimental_btn,
|
||||
is_metric_toggle,
|
||||
@@ -35,6 +37,7 @@ class TogglesLayoutMici(NavScroller):
|
||||
|
||||
# Toggle lists
|
||||
self._refresh_toggles = (
|
||||
("RadarTracks", radar_tracks),
|
||||
("ExperimentalMode", self._experimental_btn),
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
|
||||
@@ -11,6 +11,7 @@ from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
|
||||
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracksStatus
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
@@ -134,9 +135,11 @@ class BookmarkIcon(Widget):
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView):
|
||||
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
|
||||
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD,
|
||||
radar_tracks_settings_callback=None):
|
||||
super().__init__("camerad", stream_type)
|
||||
self._bookmark_callback = bookmark_callback
|
||||
self._radar_tracks_status = RadarTracksStatus(radar_tracks_settings_callback)
|
||||
self._set_placeholder_color(rl.BLACK)
|
||||
|
||||
self.device_camera: DeviceCameraConfig | None = None
|
||||
@@ -170,6 +173,13 @@ class AugmentedRoadView(CameraView):
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
|
||||
if ui_state.sm.updated["liveTracks"]:
|
||||
self._radar_tracks_status.update(
|
||||
ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks, ui_state.sm["carState"].vEgo,
|
||||
)
|
||||
elif not ui_state.sm.alive["liveTracks"]:
|
||||
self._radar_tracks_status.reset()
|
||||
|
||||
# update offroad label
|
||||
if ui_state.panda_type == log.PandaState.PandaType.unknown:
|
||||
self._offroad_label.set_text("system booting")
|
||||
@@ -179,6 +189,9 @@ class AugmentedRoadView(CameraView):
|
||||
self._offroad_label.set_text("start the car to\nuse sunnypilot")
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
if self._radar_tracks_status.handle_mouse(mouse_pos):
|
||||
return
|
||||
|
||||
# Don't trigger click callback if bookmark was triggered
|
||||
if not self._bookmark_icon.interacting():
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
@@ -221,6 +234,8 @@ class AugmentedRoadView(CameraView):
|
||||
# Fade out bottom of overlays for looks
|
||||
rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
self._radar_tracks_status.render(self._content_rect)
|
||||
|
||||
alert_to_render, not_animating_out = self._alert_renderer.will_render()
|
||||
|
||||
# Hide DMoji when disengaged unless AlwaysOnDM is enabled
|
||||
|
||||
@@ -14,6 +14,7 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP, ModelRendererSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors
|
||||
|
||||
CLIP_MARGIN = 500
|
||||
MIN_DRAW_DISTANCE = 10.0
|
||||
@@ -38,7 +39,6 @@ LANE_LINE_COLORS = {
|
||||
**LANE_LINE_COLORS_SP,
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class ModelPoints:
|
||||
raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32))
|
||||
@@ -47,9 +47,12 @@ class ModelPoints:
|
||||
|
||||
@dataclass
|
||||
class LeadVehicle:
|
||||
glow: list[float] = field(default_factory=list)
|
||||
chevron: list[float] = field(default_factory=list)
|
||||
glow: list[tuple[float, float]] = field(default_factory=list)
|
||||
chevron: list[tuple[float, float]] = field(default_factory=list)
|
||||
fill_alpha: int = 0
|
||||
position: tuple[float, float] | None = None
|
||||
radar_track_id: int = -1
|
||||
radar: bool = False
|
||||
|
||||
|
||||
class ModelRenderer(Widget, ModelRendererSP):
|
||||
@@ -132,11 +135,12 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
model = sm['modelV2']
|
||||
radar_state = sm['radarState'] if sm.valid['radarState'] else None
|
||||
lead_one = radar_state.leadOne if radar_state else None
|
||||
render_lead_indicator = self._longitudinal_control and radar_state is not None
|
||||
render_lead_indicator = ui_state.radar_tracks != 0 and radar_state is not None
|
||||
|
||||
# Update model data when needed
|
||||
model_updated = sm.updated['modelV2']
|
||||
if model_updated or sm.updated['radarState'] or self._transform_dirty:
|
||||
transform_updated = self._transform_dirty
|
||||
if model_updated or sm.updated['radarState'] or transform_updated:
|
||||
if model_updated:
|
||||
self._update_raw_points(model)
|
||||
|
||||
@@ -154,8 +158,27 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
# if render_lead_indicator and radar_state:
|
||||
# self._draw_lead_indicator()
|
||||
if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
|
||||
if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or
|
||||
not self.radar_tracks.projection_initialized):
|
||||
self.radar_tracks.update_radar_tracks(
|
||||
sm['liveTracks'], self._map_to_screen, self._path_offset_z,
|
||||
)
|
||||
highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {}
|
||||
matched_positions = self.radar_tracks.draw_cached_radar_tracks(
|
||||
screen_offset=(self._rect.x, self._rect.y),
|
||||
highlighted_tracks=highlighted_tracks,
|
||||
)
|
||||
if render_lead_indicator:
|
||||
draw_radar_lead_connectors(
|
||||
self._lead_vehicles, matched_positions, highlighted_tracks,
|
||||
screen_offset=(self._rect.x, self._rect.y),
|
||||
)
|
||||
else:
|
||||
self.radar_tracks.clear_projection()
|
||||
|
||||
if render_lead_indicator:
|
||||
self._draw_lead_indicator()
|
||||
|
||||
def _update_raw_points(self, model):
|
||||
"""Update raw 3D points from model data"""
|
||||
@@ -185,7 +208,11 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
|
||||
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
|
||||
if point:
|
||||
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
lead_vehicle.position = lead_vehicle.chevron[1]
|
||||
lead_vehicle.radar_track_id = int(lead_data.radarTrackId)
|
||||
lead_vehicle.radar = lead_data.radar
|
||||
self._lead_vehicles[i] = lead_vehicle
|
||||
|
||||
def _update_model(self, lead, path_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
@@ -380,8 +407,10 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
if not lead.glow or not lead.chevron:
|
||||
continue
|
||||
|
||||
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
|
||||
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
|
||||
offset_glow = [(x + self._rect.x, y + self._rect.y) for x, y in lead.glow]
|
||||
offset_chevron = [(x + self._rect.x, y + self._rect.y) for x, y in lead.chevron]
|
||||
rl.draw_triangle_fan(offset_glow, len(offset_glow), rl.Color(218, 202, 37, 255))
|
||||
rl.draw_triangle_fan(offset_chevron, len(offset_chevron), rl.Color(201, 34, 49, lead.fill_alpha))
|
||||
|
||||
@staticmethod
|
||||
def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
|
||||
|
||||
@@ -18,6 +18,7 @@ if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.augmented_road_view import BORDER_COLORS_SP, AugmentedRoadViewSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.driver_state import DriverStateRendererSP as DriverStateRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.hud_renderer import HudRendererSP as HudRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracksStatus
|
||||
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
|
||||
|
||||
OpState = log.SelfdriveState.OpenpilotState
|
||||
@@ -39,7 +40,7 @@ INF_POINT = np.array([1000.0, 0.0, 0.0])
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
|
||||
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD, radar_tracks_settings_callback=None):
|
||||
CameraView.__init__(self, "camerad", stream_type)
|
||||
AugmentedRoadViewSP.__init__(self)
|
||||
self._set_placeholder_color(BORDER_COLORS[UIStatus.DISENGAGED])
|
||||
@@ -51,12 +52,24 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
self._matrix_cache_key = (0, 0.0, 0.0, stream_type)
|
||||
self._cached_matrix: np.ndarray | None = None
|
||||
self._content_rect = rl.Rectangle()
|
||||
self._radar_tracks_status = RadarTracksStatus(radar_tracks_settings_callback, right_margin=240) if gui_app.sunnypilot_ui() else None
|
||||
|
||||
self.model_renderer = ModelRenderer()
|
||||
self._hud_renderer = HudRenderer()
|
||||
self.alert_renderer = AlertRenderer()
|
||||
self.driver_state_renderer = DriverStateRenderer()
|
||||
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
if self._radar_tracks_status is None:
|
||||
return
|
||||
if ui_state.sm.updated["liveTracks"]:
|
||||
self._radar_tracks_status.update(
|
||||
ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks, ui_state.sm["carState"].vEgo,
|
||||
)
|
||||
elif not ui_state.sm.alive["liveTracks"]:
|
||||
self._radar_tracks_status.reset()
|
||||
|
||||
def _render(self, rect):
|
||||
# Only render when system is started to avoid invalid data access
|
||||
if not ui_state.started:
|
||||
@@ -90,6 +103,8 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
# Draw all UI overlays
|
||||
self.model_renderer.render(self._content_rect)
|
||||
AugmentedRoadViewSP.update_fade_out_bottom_overlay(self, self._content_rect)
|
||||
if self._radar_tracks_status is not None:
|
||||
self._radar_tracks_status.render(self._content_rect)
|
||||
self._hud_renderer.render(self._content_rect)
|
||||
self.alert_renderer.render(self._content_rect)
|
||||
self.driver_state_renderer.render(self._content_rect)
|
||||
@@ -103,7 +118,9 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
# Draw colored border based on driving state
|
||||
self._draw_border(rect)
|
||||
|
||||
def _handle_mouse_press(self, _):
|
||||
def _handle_mouse_press(self, mouse_pos):
|
||||
if self._radar_tracks_status is not None and self._radar_tracks_status.handle_mouse(mouse_pos):
|
||||
return
|
||||
if not self._hud_renderer.user_interacting() and self._click_callback is not None:
|
||||
self._click_callback()
|
||||
|
||||
|
||||
@@ -13,6 +13,7 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.model_renderer import ChevronMetrics, ModelRendererSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors
|
||||
|
||||
CLIP_MARGIN = 500
|
||||
MIN_DRAW_DISTANCE = 10.0
|
||||
@@ -39,9 +40,12 @@ class ModelPoints:
|
||||
|
||||
@dataclass
|
||||
class LeadVehicle:
|
||||
glow: list[float] = field(default_factory=list)
|
||||
chevron: list[float] = field(default_factory=list)
|
||||
glow: list[tuple[float, float]] = field(default_factory=list)
|
||||
chevron: list[tuple[float, float]] = field(default_factory=list)
|
||||
fill_alpha: int = 0
|
||||
position: tuple[float, float] | None = None
|
||||
radar_track_id: int = -1
|
||||
radar: bool = False
|
||||
|
||||
|
||||
class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
@@ -115,11 +119,12 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
model = sm['modelV2']
|
||||
radar_state = sm['radarState'] if sm.valid['radarState'] else None
|
||||
lead_one = radar_state.leadOne if radar_state else None
|
||||
render_lead_indicator = self._longitudinal_control and radar_state is not None
|
||||
render_lead_indicator = ui_state.radar_tracks != 0 and radar_state is not None
|
||||
|
||||
# Update model data when needed
|
||||
model_updated = sm.updated['modelV2']
|
||||
if model_updated or sm.updated['radarState'] or self._transform_dirty:
|
||||
transform_updated = self._transform_dirty
|
||||
if model_updated or sm.updated['radarState'] or transform_updated:
|
||||
if model_updated:
|
||||
self._update_raw_points(model)
|
||||
|
||||
@@ -136,6 +141,21 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
|
||||
if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or
|
||||
not self.radar_tracks.projection_initialized):
|
||||
self.radar_tracks.update_radar_tracks(
|
||||
sm['liveTracks'], self._map_to_screen, self._path_offset_z,
|
||||
)
|
||||
highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {}
|
||||
matched_positions = self.radar_tracks.draw_cached_radar_tracks(
|
||||
highlighted_tracks=highlighted_tracks,
|
||||
)
|
||||
if render_lead_indicator:
|
||||
draw_radar_lead_connectors(self._lead_vehicles, matched_positions, highlighted_tracks)
|
||||
else:
|
||||
self.radar_tracks.clear_projection()
|
||||
|
||||
if render_lead_indicator and radar_state:
|
||||
self._draw_lead_indicator()
|
||||
self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles)
|
||||
@@ -168,7 +188,11 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
|
||||
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
|
||||
if point:
|
||||
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
lead_vehicle.position = lead_vehicle.chevron[1]
|
||||
lead_vehicle.radar_track_id = int(lead_data.radarTrackId)
|
||||
lead_vehicle.radar = lead_data.radar
|
||||
self._lead_vehicles[i] = lead_vehicle
|
||||
|
||||
def _update_model(self, lead, path_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
|
||||
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
import pyray as rl
|
||||
from openpilot.selfdrive.ui.ui_state import UIStatus
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
|
||||
|
||||
LANE_LINE_COLORS_SP = {
|
||||
UIStatus.LAT_ONLY: rl.Color(0, 255, 64, 255),
|
||||
@@ -17,3 +18,4 @@ LANE_LINE_COLORS_SP = {
|
||||
class ModelRendererSP:
|
||||
def __init__(self):
|
||||
self.rainbow_path = RainbowPath()
|
||||
self.radar_tracks = RadarTracks()
|
||||
|
||||
@@ -6,9 +6,12 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
|
||||
|
||||
|
||||
class ModelRendererSP:
|
||||
def __init__(self):
|
||||
self.rainbow_path = RainbowPath()
|
||||
self.chevron_metrics = ChevronMetrics()
|
||||
self.radar_tracks = RadarTracks()
|
||||
|
||||
|
||||
@@ -0,0 +1,339 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
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.
|
||||
"""
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import pyray as rl
|
||||
from opendbc.car.hyundai.radar_interface import RADAR_235_248, RADAR_3A5_3C4
|
||||
from openpilot.system.ui.lib.application import FontWeight
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
|
||||
NEUTRAL_COLOR = (255, 255, 255)
|
||||
DBC_MOVING_COLOR = (190, 125, 255)
|
||||
DBC_UNKNOWN_COLOR = (154, 168, 184)
|
||||
DBC_MOTION_STATIONARY = 1
|
||||
DBC_MOTION_MOVING = 2
|
||||
LEAD_TRACK_COLORS = (
|
||||
rl.Color(255, 215, 0, 255),
|
||||
rl.Color(255, 140, 0, 220),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProjectedRadarTrack:
|
||||
x: float
|
||||
y: float
|
||||
radius: float
|
||||
color: object
|
||||
source_index: int
|
||||
camera_object: bool
|
||||
track_id: int
|
||||
|
||||
|
||||
def is_preferred_radar_source(source) -> bool:
|
||||
return source.startAddress == RADAR_3A5_3C4.start_addr and source.endAddress == RADAR_3A5_3C4.end_addr
|
||||
|
||||
|
||||
def radar_source_sort_key(source) -> tuple[bool, int, int, int]:
|
||||
return (
|
||||
not is_preferred_radar_source(source),
|
||||
int(source.startAddress),
|
||||
int(source.endAddress),
|
||||
int(source.bus),
|
||||
)
|
||||
|
||||
|
||||
def sorted_radar_sources(live_tracks):
|
||||
return sorted(live_tracks.trackSources, key=radar_source_sort_key)
|
||||
|
||||
|
||||
def radar_track_source_index(track, sources) -> int:
|
||||
address = int(track.sourceAddress)
|
||||
bus = int(track.sourceBus)
|
||||
if address != 0:
|
||||
return next((
|
||||
index for index, source in enumerate(sources)
|
||||
if source.startAddress <= address <= source.endAddress and source.bus == bus
|
||||
), 0)
|
||||
return 0
|
||||
|
||||
|
||||
def is_camera_object_source(source) -> bool:
|
||||
return source.startAddress == RADAR_235_248.start_addr and source.endAddress == RADAR_235_248.end_addr
|
||||
|
||||
|
||||
def radar_track_source(track, sources):
|
||||
address = int(track.sourceAddress)
|
||||
bus = int(track.sourceBus)
|
||||
if address == 0:
|
||||
return None
|
||||
return next((
|
||||
source for source in sources
|
||||
if source.startAddress <= address <= source.endAddress and source.bus == bus
|
||||
), None)
|
||||
|
||||
|
||||
def radar_source_label(source) -> str:
|
||||
prefix = "CAM " if is_camera_object_source(source) else ""
|
||||
return f"{prefix}{source.startAddress:X}-{source.endAddress:X}"
|
||||
|
||||
|
||||
def draw_radar_source_marker(center: rl.Vector2, radius: float, color: rl.Color, source_index: int,
|
||||
camera_object: bool = False) -> None:
|
||||
if camera_object:
|
||||
rl.draw_poly(center, 3, radius, -90.0, color)
|
||||
return
|
||||
if source_index <= 0:
|
||||
rl.draw_circle(int(center.x), int(center.y), radius, color)
|
||||
return
|
||||
sides = (4, 3, 5, 6)[(source_index - 1) % 4]
|
||||
rotation = 45.0 if sides == 4 else -90.0
|
||||
rl.draw_poly(center, sides, radius, rotation, color)
|
||||
|
||||
|
||||
def radar_track_display(motion_state: int) -> tuple[rl.Color, bool]:
|
||||
"""Color tracks exclusively from the radar's DBC motion classification."""
|
||||
if motion_state == DBC_MOTION_STATIONARY:
|
||||
return rl.Color(*NEUTRAL_COLOR, 255), True
|
||||
if motion_state == DBC_MOTION_MOVING:
|
||||
return rl.Color(*DBC_MOVING_COLOR, 255), False
|
||||
return rl.Color(*DBC_UNKNOWN_COLOR, 255), False
|
||||
|
||||
|
||||
def radar_lead_track_colors(radar_state) -> dict[int, rl.Color]:
|
||||
highlighted_tracks = {}
|
||||
if radar_state is None:
|
||||
return highlighted_tracks
|
||||
|
||||
for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True):
|
||||
if lead.present and lead.radar and lead.radarTrackId >= 0:
|
||||
highlighted_tracks.setdefault(int(lead.radarTrackId), color)
|
||||
return highlighted_tracks
|
||||
|
||||
|
||||
def draw_radar_lead_connectors(lead_vehicles, matched_positions, highlighted_tracks, screen_offset=(0, 0)) -> None:
|
||||
for lead in lead_vehicles:
|
||||
if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions:
|
||||
continue
|
||||
|
||||
radar_position = matched_positions[lead.radar_track_id]
|
||||
lead_position = (lead.position[0] + screen_offset[0], lead.position[1] + screen_offset[1])
|
||||
if math.dist(lead_position, radar_position) < 4:
|
||||
continue
|
||||
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2,
|
||||
highlighted_tracks[lead.radar_track_id],
|
||||
)
|
||||
|
||||
|
||||
def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple[str, str, str, str, str, str]:
|
||||
sources = sorted_radar_sources(live_tracks)
|
||||
if not sources:
|
||||
return "", "none", "", "", "", ""
|
||||
|
||||
range_text = "\n".join(radar_source_label(source) for source in sources)
|
||||
count_text = "\n".join(str(source.trackCount) for source in sources)
|
||||
motion_states = [int(track.motionState) for track in live_tracks.points]
|
||||
|
||||
moving_count = sum(state == DBC_MOTION_MOVING for state in motion_states)
|
||||
stationary_count = sum(state == DBC_MOTION_STATIONARY for state in motion_states)
|
||||
unknown_count = len(motion_states) - moving_count - stationary_count
|
||||
return range_text, count_text, str(moving_count), str(stationary_count), str(unknown_count), ""
|
||||
|
||||
|
||||
class RadarTracksStatus:
|
||||
HORIZONTAL_PADDING = 8
|
||||
COLUMN_GAP = 8
|
||||
SOURCE_MARKER_WIDTH = 18
|
||||
|
||||
def __init__(self, settings_callback=None, right_margin: int = 12):
|
||||
self._settings_callback = settings_callback
|
||||
self._right_margin = right_margin
|
||||
self._rect = rl.Rectangle()
|
||||
text_args = {
|
||||
"font_size": 26,
|
||||
"font_weight": FontWeight.SEMI_BOLD,
|
||||
"alignment": rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
|
||||
"alignment_vertical": rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
|
||||
"wrap_text": False,
|
||||
}
|
||||
self._labels = (
|
||||
UnifiedLabel("", text_color=rl.Color(0, 255, 64, 255), **text_args),
|
||||
UnifiedLabel("none", text_color=rl.Color(0, 255, 64, 255), **text_args),
|
||||
UnifiedLabel("", text_color=rl.Color(*DBC_MOVING_COLOR, 255), **text_args),
|
||||
UnifiedLabel("", text_color=rl.Color(*NEUTRAL_COLOR, 255), **text_args),
|
||||
UnifiedLabel("", text_color=rl.Color(*DBC_UNKNOWN_COLOR, 255), **text_args),
|
||||
UnifiedLabel("", text_color=rl.Color(*DBC_UNKNOWN_COLOR, 255), **text_args),
|
||||
)
|
||||
self._status = ("", "none", "", "", "", "")
|
||||
self._status_colors: tuple[tuple[int, int, int], ...] = ()
|
||||
self._source_count = 0
|
||||
self._layout_key: tuple[str, str, str, str, str, str, int] | None = None
|
||||
self._column_widths = [0, 36, 0, 0, 0, 0]
|
||||
self._width = 52
|
||||
self._height = 42
|
||||
|
||||
def update(self, live_tracks, valid: bool, radar_mode: int, v_ego: float = 0.0) -> None:
|
||||
if live_tracks.radarTracksAvailable and radar_mode != 2:
|
||||
status = ("", "radar detected\ntap to enable", "", "", "", "")
|
||||
status_colors = ()
|
||||
source_count = 0
|
||||
else:
|
||||
status = format_radar_tracks_onroad_columns(live_tracks, v_ego) if valid else ("", "none", "", "", "", "")
|
||||
status_colors = (DBC_MOVING_COLOR, NEUTRAL_COLOR, DBC_UNKNOWN_COLOR, DBC_UNKNOWN_COLOR)
|
||||
source_count = len(live_tracks.trackSources) if valid else 0
|
||||
self._set_status(status, status_colors, source_count)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._set_status(("", "none", "", "", "", ""), (), 0)
|
||||
|
||||
def handle_mouse(self, mouse_pos) -> bool:
|
||||
if self._settings_callback is None or not rl.check_collision_point_rec(mouse_pos, self._rect):
|
||||
return False
|
||||
|
||||
self._settings_callback()
|
||||
return True
|
||||
|
||||
def render(self, content_rect: rl.Rectangle) -> None:
|
||||
self._update_layout(int(content_rect.width - 40))
|
||||
self._rect = rl.Rectangle(
|
||||
content_rect.x + content_rect.width - self._width - self._right_margin,
|
||||
content_rect.y + 8,
|
||||
self._width,
|
||||
self._height,
|
||||
)
|
||||
rl.draw_rectangle_rounded(self._rect, 0.5, 8, rl.Color(0, 0, 0, 170))
|
||||
x = self._rect.x + self.HORIZONTAL_PADDING
|
||||
active_columns = [
|
||||
(column_index, label, width)
|
||||
for column_index, (label, width) in enumerate(zip(self._labels, self._column_widths, strict=True))
|
||||
if width
|
||||
]
|
||||
for active_index, (column_index, label, width) in enumerate(active_columns):
|
||||
label_x = x
|
||||
label_width = width
|
||||
if column_index == 0 and self._source_count:
|
||||
marker_height = (self._rect.height - 10) / self._source_count
|
||||
for source_index in range(self._source_count):
|
||||
draw_radar_source_marker(
|
||||
rl.Vector2(x + 6, self._rect.y + 5 + marker_height * (source_index + 0.5)),
|
||||
5.0, rl.Color(0, 255, 64, 255), source_index,
|
||||
)
|
||||
label_x += self.SOURCE_MARKER_WIDTH
|
||||
label_width -= self.SOURCE_MARKER_WIDTH
|
||||
label.render(rl.Rectangle(label_x, self._rect.y + 5, label_width, self._rect.height - 10))
|
||||
x += width + (self.COLUMN_GAP if active_index < len(active_columns) - 1 else 0)
|
||||
|
||||
def _set_status(self, status: tuple[str, str, str, str, str, str],
|
||||
status_colors: tuple[tuple[int, int, int], ...], source_count: int) -> None:
|
||||
if status == self._status and status_colors == self._status_colors and source_count == self._source_count:
|
||||
return
|
||||
|
||||
self._status = status
|
||||
self._status_colors = status_colors
|
||||
self._source_count = source_count
|
||||
for label, text in zip(self._labels, status, strict=True):
|
||||
label.set_text(text)
|
||||
for label, color in zip(self._labels[2:], status_colors, strict=False):
|
||||
label.set_text_color(rl.Color(*color, 255))
|
||||
self._layout_key = None
|
||||
|
||||
def _update_layout(self, max_inner_width: int) -> None:
|
||||
layout_key = (*self._status, max_inner_width)
|
||||
if layout_key == self._layout_key:
|
||||
return
|
||||
|
||||
for label in self._labels:
|
||||
label.get_content_height(max_inner_width)
|
||||
self._column_widths = [
|
||||
math.ceil(label.text_width) if text else 0
|
||||
for label, text in zip(self._labels, self._status, strict=True)
|
||||
]
|
||||
if self._column_widths[0] and self._source_count:
|
||||
self._column_widths[0] += self.SOURCE_MARKER_WIDTH
|
||||
self._column_widths[1] = max(36, self._column_widths[1])
|
||||
for index in (2, 3, 4):
|
||||
if self._column_widths[index]:
|
||||
self._column_widths[index] = max(36, self._column_widths[index])
|
||||
active_widths = [width for width in self._column_widths if width]
|
||||
inner_width = sum(active_widths) + self.COLUMN_GAP * (len(active_widths) - 1)
|
||||
self._width = inner_width + self.HORIZONTAL_PADDING * 2
|
||||
self._height = max(
|
||||
42,
|
||||
*(label.get_content_height(max(width, 1)) + 10
|
||||
for label, width in zip(self._labels, self._column_widths, strict=True) if width),
|
||||
)
|
||||
self._layout_key = layout_key
|
||||
|
||||
|
||||
class RadarTracks:
|
||||
def __init__(self):
|
||||
self._projected_tracks: tuple[ProjectedRadarTrack, ...] = ()
|
||||
self._projection_initialized = False
|
||||
|
||||
@property
|
||||
def projection_initialized(self) -> bool:
|
||||
return self._projection_initialized
|
||||
|
||||
def clear_projection(self) -> None:
|
||||
self._projected_tracks = ()
|
||||
self._projection_initialized = False
|
||||
|
||||
def update_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7) -> None:
|
||||
projected_tracks = []
|
||||
sources = sorted_radar_sources(live_tracks)
|
||||
|
||||
for track in live_tracks.points:
|
||||
d_rel, y_rel, v_rel = track.dRel, track.yRel, track.vRel
|
||||
if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel)):
|
||||
continue
|
||||
|
||||
motion_state = int(track.motionState)
|
||||
if motion_state not in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING):
|
||||
continue
|
||||
|
||||
pt = map_to_screen(d_rel, -y_rel, path_offset_z)
|
||||
if pt is None:
|
||||
continue
|
||||
|
||||
color, stationary = radar_track_display(motion_state)
|
||||
radius = max(1, track_size - 5) if stationary else track_size
|
||||
source = radar_track_source(track, sources)
|
||||
projected_tracks.append(ProjectedRadarTrack(
|
||||
x=pt[0],
|
||||
y=pt[1],
|
||||
radius=radius,
|
||||
color=color,
|
||||
source_index=radar_track_source_index(track, sources),
|
||||
camera_object=source is not None and is_camera_object_source(source),
|
||||
track_id=int(track.trackId),
|
||||
))
|
||||
|
||||
self._projected_tracks = tuple(projected_tracks)
|
||||
self._projection_initialized = True
|
||||
|
||||
def draw_cached_radar_tracks(self, screen_offset=(0, 0), highlighted_tracks=None):
|
||||
highlighted_tracks = highlighted_tracks or {}
|
||||
highlighted_positions = {}
|
||||
|
||||
for track in self._projected_tracks:
|
||||
x, y = track.x + screen_offset[0], track.y + screen_offset[1]
|
||||
highlight_color = highlighted_tracks.get(track.track_id)
|
||||
if highlight_color is not None:
|
||||
center = rl.Vector2(int(x), int(y))
|
||||
rl.draw_ring(center, track.radius + 2, track.radius + 5, 0, 360, 24, highlight_color)
|
||||
highlighted_positions[track.track_id] = (x, y)
|
||||
draw_radar_source_marker(
|
||||
rl.Vector2(x, y), track.radius, track.color, track.source_index, track.camera_object,
|
||||
)
|
||||
|
||||
return highlighted_positions
|
||||
|
||||
def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7, screen_offset=(0, 0), v_ego=0.0,
|
||||
highlighted_tracks=None):
|
||||
self.update_radar_tracks(live_tracks, map_to_screen, path_offset_z, track_size)
|
||||
return self.draw_cached_radar_tracks(screen_offset, highlighted_tracks)
|
||||
@@ -0,0 +1,324 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from opendbc.car.structs import car
|
||||
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad import radar_tracks
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, format_radar_tracks_onroad_columns, \
|
||||
radar_lead_track_colors, radar_track_display
|
||||
|
||||
|
||||
def color_tuple(color):
|
||||
return color.r, color.g, color.b, color.a
|
||||
|
||||
|
||||
def test_dbc_motion_colors():
|
||||
assert color_tuple(radar_track_display(2)[0]) == (190, 125, 255, 255)
|
||||
assert not radar_track_display(2)[1]
|
||||
assert color_tuple(radar_track_display(1)[0]) == (255, 255, 255, 255)
|
||||
assert radar_track_display(1)[1]
|
||||
|
||||
|
||||
def test_unknown_dbc_motion_uses_neutral_dbc_color():
|
||||
color, stationary = radar_track_display(0)
|
||||
|
||||
assert color_tuple(color) == (*radar_tracks.DBC_UNKNOWN_COLOR, 255)
|
||||
assert not stationary
|
||||
|
||||
|
||||
def test_radar_lead_track_colors_only_highlight_radar_matches():
|
||||
radar_state = SimpleNamespace(
|
||||
leadOne=SimpleNamespace(present=True, radar=True, radarTrackId=7),
|
||||
leadTwo=SimpleNamespace(present=True, radar=False, radarTrackId=9),
|
||||
)
|
||||
|
||||
colors = radar_lead_track_colors(radar_state)
|
||||
|
||||
assert list(colors) == [7]
|
||||
assert color_tuple(colors[7]) == color_tuple(radar_tracks.LEAD_TRACK_COLORS[0])
|
||||
|
||||
|
||||
def test_draw_radar_lead_connectors_applies_screen_offset(monkeypatch):
|
||||
lead = SimpleNamespace(radar=True, position=(10, 20), radar_track_id=7)
|
||||
color = radar_tracks.LEAD_TRACK_COLORS[0]
|
||||
drawn = []
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_line_ex",
|
||||
lambda start, end, width, line_color: drawn.append(
|
||||
((start.x, start.y), (end.x, end.y), width, color_tuple(line_color))
|
||||
),
|
||||
)
|
||||
|
||||
draw_radar_lead_connectors(
|
||||
[lead], {7: (120, 30)}, {7: color}, screen_offset=(100, 5),
|
||||
)
|
||||
|
||||
assert drawn == [((110, 25), (120, 30), 2, color_tuple(color))]
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_none():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("", "none", "", "", "", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_range_and_count():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 2}]
|
||||
points = live_tracks.init("points", 2)
|
||||
points[0].motionState = 2
|
||||
points[1].motionState = 1
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("3A5-3C4", "2", "1", "1", "0", "")
|
||||
|
||||
|
||||
def test_format_camera_objects_are_not_labeled_as_radar():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x235, "endAddress": 0x248, "bus": 1, "trackCount": 3}]
|
||||
points = live_tracks.init("points", 3)
|
||||
for point in points:
|
||||
point.motionState = 2
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("CAM 235-248", "3", "3", "0", "0", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_stacks_all_ranges_with_preferred_first():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [
|
||||
{"startAddress": 0x500, "endAddress": 0x51F, "bus": 2, "trackCount": 3},
|
||||
{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 2},
|
||||
]
|
||||
points = live_tracks.init("points", 3)
|
||||
points[0].motionState = 2
|
||||
points[0].sourceAddress = 0x3A5
|
||||
points[1].motionState = 1
|
||||
points[1].sourceAddress = 0x3A6
|
||||
points[2].motionState = 0
|
||||
points[2].sourceAddress = 0x500
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == (
|
||||
"3A5-3C4\n500-51F",
|
||||
"2\n3",
|
||||
"1",
|
||||
"1",
|
||||
"1",
|
||||
"",
|
||||
)
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_shows_non_motion_source():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 4}]
|
||||
points = live_tracks.init("points", 4)
|
||||
for point, v_rel in zip(points, (-5.0, 0.2, -20.0, 5.0), strict=True):
|
||||
point.vRel = v_rel
|
||||
point.motionState = 0
|
||||
point.sourceAddress = 0x500
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == ("500-51F", "4", "0", "0", "4", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_shows_64_track_source():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x53F, "bus": 1, "trackCount": 7}]
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("500-53F", "7", "0", "0", "0", "")
|
||||
|
||||
|
||||
def test_draw_radar_tracks_applies_screen_offset(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
points = live_tracks.init("points", 1)
|
||||
points[0].dRel = 10
|
||||
points[0].yRel = 1
|
||||
points[0].vRel = 2
|
||||
points[0].motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
drawn_circles = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_circles.append((x, y, size)))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks,
|
||||
lambda d_rel, y_rel, z: (20, 30),
|
||||
path_offset_z=1.2,
|
||||
track_size=3,
|
||||
screen_offset=(100, 7),
|
||||
)
|
||||
|
||||
assert drawn_circles == [(120, 37, 3)]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_hides_unknown_motion(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -5
|
||||
point.motionState = 0
|
||||
drawn_colors = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_colors.append(color_tuple(color)))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2)
|
||||
|
||||
assert drawn_colors == []
|
||||
|
||||
|
||||
def test_draw_radar_tracks_hides_unknown_motion_from_other_source(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -5
|
||||
point.motionState = 0
|
||||
point.sourceAddress = 0x500
|
||||
drawn_circles = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda *args: drawn_circles.append(args))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2,
|
||||
)
|
||||
|
||||
assert drawn_circles == []
|
||||
|
||||
|
||||
def test_draw_radar_tracks_uses_source_shapes_with_preferred_circle(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [
|
||||
{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 1},
|
||||
{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 1},
|
||||
]
|
||||
points = live_tracks.init("points", 2)
|
||||
for point, address in zip(points, (0x500, 0x3A5), strict=True):
|
||||
point.dRel = address
|
||||
point.yRel = 1
|
||||
point.vRel = 2
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
point.sourceAddress = address
|
||||
point.sourceBus = 1
|
||||
|
||||
circles = []
|
||||
polygons = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, radius, color: circles.append((x, radius)))
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_poly",
|
||||
lambda center, sides, radius, rotation, color: polygons.append((center.x, sides, radius, rotation)),
|
||||
)
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2, track_size=6,
|
||||
)
|
||||
|
||||
assert circles == [(0x3A5, 6)]
|
||||
assert polygons == [(0x500, 4, 6, 45.0)]
|
||||
|
||||
|
||||
def test_draw_camera_objects_uses_triangle(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x235, "endAddress": 0x248, "bus": 1, "trackCount": 1}]
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 25
|
||||
point.yRel = 1
|
||||
point.vRel = 2
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
point.sourceAddress = 0x235
|
||||
point.sourceBus = 1
|
||||
polygons = []
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_poly",
|
||||
lambda center, sides, radius, rotation, color: polygons.append((center.x, sides, radius, rotation)),
|
||||
)
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2, track_size=6,
|
||||
)
|
||||
|
||||
assert polygons == [(25, 3, 6, -90.0)]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -20
|
||||
point.motionState = 1
|
||||
drawn_sizes = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, track_size=6, v_ego=20,
|
||||
)
|
||||
|
||||
assert drawn_sizes == [1]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_keeps_matched_speed_dots_large(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = 0.5
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
drawn_sizes = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, track_size=6, v_ego=20,
|
||||
)
|
||||
|
||||
assert drawn_sizes == [6]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_highlights_and_returns_matched_track(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
points = live_tracks.init("points", 2)
|
||||
for track_id, point in enumerate(points, start=10):
|
||||
point.trackId = track_id
|
||||
point.dRel = track_id
|
||||
point.yRel = 1
|
||||
point.vRel = 2
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
|
||||
highlight_color = radar_tracks.rl.Color(255, 215, 0, 255)
|
||||
drawn_rings = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda *args: None)
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl,
|
||||
"draw_ring",
|
||||
lambda center, inner, outer, start, end, segments, color: drawn_rings.append(
|
||||
((center.x, center.y), inner, outer, color_tuple(color))
|
||||
),
|
||||
)
|
||||
|
||||
matched_positions = radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2,
|
||||
screen_offset=(100, 7), highlighted_tracks={11: highlight_color},
|
||||
)
|
||||
|
||||
assert drawn_rings == [((111, 37), 9, 12, (255, 215, 0, 255))]
|
||||
assert matched_positions == {11: (111, 37)}
|
||||
|
||||
|
||||
def test_cached_radar_tracks_only_reproject_on_update(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.trackId = 7
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = 0
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
projected = []
|
||||
drawn = []
|
||||
|
||||
def map_to_screen(d_rel, y_rel, z):
|
||||
projected.append((d_rel, y_rel, z))
|
||||
return (20, 30)
|
||||
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_circle",
|
||||
lambda x, y, radius, color: drawn.append((x, y)),
|
||||
)
|
||||
renderer = radar_tracks.RadarTracks()
|
||||
renderer.update_radar_tracks(live_tracks, map_to_screen, path_offset_z=1.2)
|
||||
renderer.draw_cached_radar_tracks(screen_offset=(100, 7))
|
||||
renderer.draw_cached_radar_tracks(screen_offset=(200, 9))
|
||||
|
||||
assert projected == [(10, -1, 1.2)]
|
||||
assert drawn == [(120, 37), (220, 39)]
|
||||
@@ -44,6 +44,7 @@ class UIStateSP:
|
||||
self.custom_interactive_timeout: int = 0
|
||||
self.developer_ui = None
|
||||
self.hide_v_ego_ui: bool = False
|
||||
self.radar_tracks: int = self.params.get("RadarTracks", return_default=True)
|
||||
self.onroad_brightness: int = 0
|
||||
self.onroad_brightness_timer: int = 0
|
||||
self.onroad_brightness_timer_param: int = 0
|
||||
@@ -152,6 +153,7 @@ class UIStateSP:
|
||||
self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)
|
||||
self.developer_ui = self.params.get("DevUIInfo")
|
||||
self.hide_v_ego_ui = self.params.get_bool("HideVEgoUI")
|
||||
self.radar_tracks = self.params.get("RadarTracks", return_default=True)
|
||||
self.onroad_brightness = int(float(self.params.get("OnroadScreenOffBrightness", return_default=True)))
|
||||
self.onroad_brightness_timer_param = self.params.get("OnroadScreenOffTimer", return_default=True)
|
||||
self.rainbow_path = self.params.get_bool("RainbowMode")
|
||||
|
||||
@@ -63,6 +63,7 @@ class UIState(UIStateSP):
|
||||
"liveParameters",
|
||||
"testJoystick",
|
||||
"rawAudioData",
|
||||
"liveTracks",
|
||||
] + self.sm_services_ext
|
||||
)
|
||||
|
||||
|
||||
@@ -109,6 +109,8 @@ def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
|
||||
def initialize_params(params) -> list[dict[str, Any]]:
|
||||
keys: list = []
|
||||
|
||||
keys.append("RadarTracks")
|
||||
|
||||
# hyundai
|
||||
keys.extend([
|
||||
"HyundaiLongitudinalTuning",
|
||||
|
||||
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
import time
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
import numpy as np
|
||||
from openpilot.cereal import log, custom
|
||||
|
||||
from opendbc.car import structs
|
||||
@@ -20,6 +21,10 @@ from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import Lat
|
||||
|
||||
|
||||
class ControlsExt(ModelStateBase):
|
||||
_radar_tracks_active: bool
|
||||
_radar_tracks_cache: tuple[dict[str, int | float], ...]
|
||||
_radar_tracks_cache_initialized: bool
|
||||
|
||||
def __init__(self, CP: structs.CarParams, params: Params):
|
||||
ModelStateBase.__init__(self)
|
||||
self.CP = CP
|
||||
@@ -31,8 +36,14 @@ 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']
|
||||
self._reset_radar_track_cache()
|
||||
|
||||
def _reset_radar_track_cache(self) -> None:
|
||||
self._radar_tracks_active = False
|
||||
self._radar_tracks_cache = ()
|
||||
self._radar_tracks_cache_initialized = False
|
||||
|
||||
def initialize_lateral_control(self, lac, CI, dt):
|
||||
enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
|
||||
@@ -85,11 +96,65 @@ class ControlsExt(ModelStateBase):
|
||||
_lead.radar = src.radar
|
||||
_lead.radarTrackId = src.radarTrackId
|
||||
|
||||
@staticmethod
|
||||
def build_radar_track_data(live_tracks, valid: bool, model=None,
|
||||
model_valid: bool = False) -> tuple[bool, tuple[dict[str, int | float], ...]]:
|
||||
radar_tracks_active = valid and len(live_tracks.trackSources) > 0
|
||||
if not radar_tracks_active:
|
||||
return False, ()
|
||||
|
||||
source_tracks = [track for track in live_tracks.points if track.motionState in (1, 2)]
|
||||
if not source_tracks:
|
||||
return True, ()
|
||||
|
||||
path_x = np.asarray(model.position.x, dtype=float) if model_valid else np.empty(0)
|
||||
path_y = np.asarray(model.position.y, dtype=float) if model_valid else np.empty(0)
|
||||
path_valid = len(path_x) >= 2 and len(path_x) == len(path_y) and np.all(np.isfinite(path_x)) and np.all(np.isfinite(path_y))
|
||||
radar_tracks = []
|
||||
for src in source_tracks:
|
||||
center_y = float(np.interp(src.dRel, path_x, path_y)) if path_valid else 0.0
|
||||
radar_tracks.append({
|
||||
"trackId": int(src.trackId),
|
||||
"dRel": float(src.dRel),
|
||||
"yRel": float(src.yRel + center_y),
|
||||
"vRel": float(src.vRel),
|
||||
"motionState": int(src.motionState),
|
||||
"age": int(src.trackAge),
|
||||
})
|
||||
return radar_tracks_active, tuple(radar_tracks)
|
||||
|
||||
@staticmethod
|
||||
def set_radar_track_data(CC_SP: custom.CarControlSP, active: bool,
|
||||
radar_tracks: tuple[dict[str, int | float], ...]) -> None:
|
||||
CC_SP.radarTracksActive = active
|
||||
CC_SP.radarTracks = radar_tracks
|
||||
|
||||
@classmethod
|
||||
def get_radar_track_data(cls, CC_SP: custom.CarControlSP, live_tracks, valid: bool,
|
||||
model=None, model_valid: bool = False) -> None:
|
||||
active, radar_tracks = cls.build_radar_track_data(live_tracks, valid, model, model_valid)
|
||||
cls.set_radar_track_data(CC_SP, active, radar_tracks)
|
||||
|
||||
def update_radar_track_cache(self, live_tracks, valid: bool, model=None,
|
||||
model_valid: bool = False, inputs_updated: bool = True) -> None:
|
||||
if self._radar_tracks_cache_initialized and not inputs_updated:
|
||||
return
|
||||
|
||||
self._radar_tracks_active, self._radar_tracks_cache = self.build_radar_track_data(
|
||||
live_tracks, valid, model, model_valid,
|
||||
)
|
||||
self._radar_tracks_cache_initialized = True
|
||||
|
||||
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.update_radar_track_cache(
|
||||
sm['liveTracks'], sm.valid['liveTracks'], sm['modelV2'], sm.valid['modelV2'],
|
||||
sm.updated['liveTracks'] or sm.updated['modelV2'],
|
||||
)
|
||||
self.set_radar_track_data(CC_SP, self._radar_tracks_active, self._radar_tracks_cache)
|
||||
|
||||
# MADS state
|
||||
mads_src = sm['selfdriveStateSP'].mads
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
|
||||
|
||||
|
||||
def make_live_tracks(motion_states: tuple[int, ...], with_source: bool = True):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
points = live_tracks.init("points", len(motion_states))
|
||||
for track_id, (point, motion_state) in enumerate(zip(points, motion_states, strict=True), start=7):
|
||||
point.trackId = track_id
|
||||
point.dRel = 12
|
||||
point.yRel = -3
|
||||
point.vRel = 1
|
||||
point.motionState = motion_state
|
||||
if with_source:
|
||||
live_tracks.trackSources = [{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1,
|
||||
"trackCount": len(motion_states)}]
|
||||
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((2,), with_source=False), valid=True)
|
||||
|
||||
assert not cc_sp.radarTracksActive
|
||||
assert len(cc_sp.radarTracks) == 0
|
||||
|
||||
|
||||
def test_active_radar_tracks_copy_only_classified_points():
|
||||
cc_sp = custom.CarControlSP.new_message()
|
||||
|
||||
ControlsExt.get_radar_track_data(cc_sp, make_live_tracks((2, 1, 0, 255)), valid=True)
|
||||
|
||||
assert cc_sp.radarTracksActive
|
||||
assert [track.trackId for track in cc_sp.radarTracks] == [7, 8]
|
||||
assert [track.motionState for track in cc_sp.radarTracks] == [2, 1]
|
||||
|
||||
|
||||
def test_radar_tracks_are_positioned_relative_to_curved_model_path():
|
||||
cc_sp = custom.CarControlSP.new_message()
|
||||
live_tracks = make_live_tracks((2, 2))
|
||||
live_tracks.points[0].dRel = 20
|
||||
live_tracks.points[0].yRel = -3
|
||||
live_tracks.points[1].dRel = 20
|
||||
live_tracks.points[1].yRel = -6.5
|
||||
model = messaging.new_message("modelV2").modelV2
|
||||
model.position.x = [0, 10, 20]
|
||||
model.position.y = [0, 1, 3]
|
||||
|
||||
ControlsExt.get_radar_track_data(cc_sp, live_tracks, valid=True, model=model, model_valid=True)
|
||||
|
||||
assert cc_sp.radarTracks[0].yRel == 0
|
||||
assert cc_sp.radarTracks[1].yRel == -3.5
|
||||
|
||||
|
||||
def test_radar_track_cache_only_rebuilds_when_inputs_update():
|
||||
controls_ext = object.__new__(ControlsExt)
|
||||
controls_ext._reset_radar_track_cache()
|
||||
live_tracks = make_live_tracks((2,))
|
||||
|
||||
controls_ext.update_radar_track_cache(live_tracks, valid=True, inputs_updated=True)
|
||||
assert controls_ext._radar_tracks_cache[0]["dRel"] == 12
|
||||
|
||||
live_tracks.points[0].dRel = 30
|
||||
controls_ext.update_radar_track_cache(live_tracks, valid=True, inputs_updated=False)
|
||||
assert controls_ext._radar_tracks_cache[0]["dRel"] == 12
|
||||
|
||||
controls_ext.update_radar_track_cache(live_tracks, valid=True, inputs_updated=True)
|
||||
assert controls_ext._radar_tracks_cache[0]["dRel"] == 30
|
||||
@@ -0,0 +1,62 @@
|
||||
# Hyundai/Kia/Genesis radar range matrix
|
||||
|
||||
This is a forum-ready summary of the routes analyzed for the sunnypilot radar
|
||||
tracks work. It describes observed protocol families, not guaranteed equipment
|
||||
for every model year, market, trim, or harness. A range can be present but empty
|
||||
when the radar is inactive. Raw bus number is also not a layout identifier:
|
||||
follow the sustained logical A-CAN source and reject brief forwarded copies.
|
||||
|
||||
Status terms:
|
||||
|
||||
- **Confirmed**: complete range, checksum/cadence, and tracked-object layout
|
||||
were validated across route data.
|
||||
- **Observed/optional**: verified on at least one qualifying route, but not
|
||||
every route or trim carrying the main front-radar family.
|
||||
- **Candidate**: source association or exact semantics remain incomplete.
|
||||
- **Not radar**: camera or fused-output traffic overlapping radar-like ranges.
|
||||
|
||||
## Platform matrix
|
||||
|
||||
| Platform or platform group | Front tracked-object range | Front layout | Additional radar traffic | Camera/fused traffic that is not another radar | Status and notes |
|
||||
| --- | --- | --- | --- | --- | --- |
|
||||
| Kia K7 2017; older Hyundai Ioniq; sampled Ioniq PHEV | `0x500–0x53F` | 64 slots, 8 B each, about 20 Hz | `0x4E0`, `0x4E1`, `0x4E3` status/path messages | Legacy `0x238–0x24F` may coexist on applicable cars | **Confirmed** 64-slot legacy/Delphi-ESR dialect. |
|
||||
| Kia Niro EV first generation; legacy Hyundai Palisade; Sonata; Sonata Hybrid | `0x500–0x51F` | 32 slots, 8 B each, about 20 Hz | `0x4E0–0x4E5` status/build/health/alignment family | Legacy `0x238–0x255` or related camera candidates may coexist by platform | **Confirmed** 32-slot legacy dialect. The upper `0x520–0x53F` bank is absent. |
|
||||
| Hyundai Kona EV 2022; Kia Ceed PHEV | `0x602–0x617` | Two packed tracks in implemented messages | None established | None established | **Observed/candidate** legacy front family; the relationship between `0x602–0x611` and `0x612–0x617` remains incomplete. |
|
||||
| Hyundai Elantra HEV 2024 | `0x210–0x21F` | Detailed dialect, 32 tracks in 16 × 32 B messages, 20 Hz | No paired corner family established | Platform-dependent camera traffic | **Confirmed** detailed `210` front radar. |
|
||||
| Hyundai Ioniq 5; Hyundai Palisade 2023; Kia EV6, older HDA2 samples | `0x210–0x21F` | Detailed dialect, 32 tracks | Right-front candidate: `0x240–0x24F` objects plus `0x270–0x277` compact records. Left-front candidate: `0x278–0x287` objects plus `0x288–0x28F` compact records. | Isolated `0x235` traffic is not a complete camera-object family | **Confirmed** front radar. Paired front corners are **observed/optional** only on qualifying routes; A=right and B=left are strongly inferred but still need physical occlusion confirmation. |
|
||||
| Hyundai Santa Cruz 2025; Tucson 4th generation; Tucson HEV 2025; Kia Sportage 5th generation | `0x210–0x21F` | Compact dialect, 32 tracks | No corner family established in checked routes | Platform-dependent camera traffic | **Confirmed** compact `210` front radar. Compact and detailed `210` share core kinematics but use different lifecycle fields. |
|
||||
| Hyundai Ioniq 6 | `0x3A5–0x3C4` | Common 32-track layout, 24 B each, 20 Hz | No independent corner range established | `0x235–0x248` camera objects and `0x360–0x366` camera lane/path may be present | **Confirmed** common `3A5` front radar. |
|
||||
| Hyundai Kona/Kona HEV/Kona EV 2nd generation; Santa Fe HEV 5th generation; Sonata 2024 | `0x3A5–0x3C4` | Common 32-track layout | No independent corner range established | Platform-dependent `0x235`, `0x360`, and CCNC `0x162` fused traffic | **Confirmed** common `3A5` front radar. CCNC/HDA generation does not imply rich radar extensions. |
|
||||
| Kia Carnival HEV 2026; EV6 2025; K4 2025; K5 2025; Niro EV/HEV 2nd generation | `0x3A5–0x3C4` | Common 32-track layout | No independent corner range established | Platform-dependent `0x235`, `0x360`, and CCNC `0x162` fused traffic | **Confirmed** common `3A5` front radar. Bus 0/1 varies by architecture; a bus-2 copy can be forwarding. |
|
||||
| Kia EV9; Hyundai Ioniq 9 | `0x3A5–0x3C4` | Rich 32-track layout with optional width, length, absolute speed, and orientation | `0x3D0–0x3D4` synchronized 32-position auxiliary scan array | `0x235–0x248` camera objects; `0x360–0x366` camera lane/path; CCNC `0x162` fused traffic | **Confirmed** rich `3A5`; `3D0` is **observed/optional** radar auxiliary traffic. Former `3D0` speed/azimuth interpretations were falsified, so it is counted but not projected as geometry. |
|
||||
| Kia Sportage HEV 2026 forum sample | Overlaps `0x3A5–0x3C4` addresses | Payload is incompatible with the common `3A5` layout | Unknown | Unknown | **Excluded** from the common decoder; address overlap alone is not layout compatibility. |
|
||||
|
||||
## Other observed ranges
|
||||
|
||||
| Range | Applicable examples | Classification |
|
||||
| --- | --- | --- |
|
||||
| `0x235–0x248`, 32 B at about 33 Hz | Newer CAN-FD platforms, including rich EV9/Ioniq 9 captures | **Not radar:** forward-camera object list with class, motion, ID, geometry, velocity, and acceleration. |
|
||||
| `0x360–0x366`, 32 B | Newer CAN-FD platforms carrying camera lane/path output | **Not radar:** forward-camera lane/path family. |
|
||||
| `0x180–0x184`, `0x1B6–0x1B9`, `0x2BB–0x2BE` | Sampled Kia Sorento/Sorento HEV and Genesis GV70 candidates | **Not radar:** forward-camera auxiliary lists according to downloaded Hyundai DBCs. |
|
||||
| `0x238–0x24F`, 8 B | Ioniq PHEV/HEV 2022, legacy Palisade, Niro PHEV candidates | **Not established as radar:** strong legacy forward-camera-perception candidate. |
|
||||
| `0x238–0x255`, 8 B, often with `0x25A–0x25E` | Custin, Elantra/Elantra HEV 2021, Sonata/Sonata Hybrid candidates | **Not established as radar:** extended legacy forward-camera-perception candidate. |
|
||||
| `CCNC_0x162` | CCNC platforms | **Not raw radar:** stock fused/cluster lead, side, and rear slots with unknown upstream fusion. |
|
||||
| Bus-9 `0x300/0x400/0x500/0x600` blocks | Historical EV6 prototype capture | **Historical candidate:** four-corner FL/FR/RL/RR prototype; not reconfirmed in the current production-route corpus. |
|
||||
|
||||
## Supported combinations observed in the corpus
|
||||
|
||||
| Combination | Front | Corners or radar auxiliary |
|
||||
| --- | --- | --- |
|
||||
| Legacy front, 32 slots | `0x500–0x51F` | `0x4E0–0x4E5` companion/status |
|
||||
| Legacy front, 64 slots | `0x500–0x53F` | `0x4E0`, `0x4E1`, `0x4E3` companion/status |
|
||||
| Legacy alternate front | `0x602–0x617` | None established |
|
||||
| CAN-FD detailed front only | `0x210–0x21F` | None established |
|
||||
| CAN-FD detailed front plus paired front corners | `0x210–0x21F` | A/right candidate `0x240/0x270` families plus B/left candidate `0x278/0x288` families |
|
||||
| CAN-FD compact front only | `0x210–0x21F` | None established |
|
||||
| CAN-FD common front | `0x3A5–0x3C4` | Camera/fused families may coexist but are not independent radar tracks |
|
||||
| CAN-FD rich front plus auxiliary scan | `0x3A5–0x3C4` | Optional `0x3D0–0x3D4` auxiliary radar array |
|
||||
|
||||
No checked route proves a combination containing both the paired
|
||||
`0x240/0x270–0x28F` corner family and `0x3D0–0x3D4`. No distinct production
|
||||
rear-radar tracked-object range has been confirmed. Rear objects in
|
||||
`CCNC_0x162` are fused outputs and do not prove a raw rear-radar range.
|
||||
@@ -0,0 +1,228 @@
|
||||
# Hyundai/Kia/Genesis radar source inventory
|
||||
|
||||
This document separates physical radar sources from camera and fused-object
|
||||
traffic. Signal-level evidence is in [SIGNALS.md](SIGNALS.md).
|
||||
The copy-ready vehicle/range summary is in
|
||||
[PLATFORM_MATRIX.md](PLATFORM_MATRIX.md).
|
||||
|
||||
## Confidence and safety rules
|
||||
|
||||
- **Confirmed layout** means that bit positions, scales, and behavior agree
|
||||
across multiple routes. It does not automatically prove the physical ECU.
|
||||
- **Strong role** means that topology, cadence, geometry, and related messages
|
||||
support the assignment, but a controlled sensor-occlusion test is still
|
||||
desirable.
|
||||
- **Candidate** means useful research data only. Candidate fields must not
|
||||
affect control or production filtering.
|
||||
- Identify a source by the complete address set, payload sizes, cadence,
|
||||
checksum behavior, and logical CAN path. An address or raw bus number alone
|
||||
is not enough.
|
||||
- A short copy on another bus is normally a forwarding echo. It is not a
|
||||
second sensor unless timestamps and payload differences prove independence.
|
||||
|
||||
## Current source families
|
||||
|
||||
| Source | Layout | Role | Current conclusion |
|
||||
| --- | --- | --- | --- |
|
||||
| `0x500–0x51F` or `0x500–0x53F` | 32 or 64 × 8 B at about 20 Hz | Front radar | Shared legacy core with a Delphi ESR extension on the 64-slot dialect. K7, older Ioniq, and Ioniq PHEV samples carry 64 slots; Niro EV, Palisade, Sonata, and Sonata Hybrid samples carry 32. Implemented as standardized `RADAR_500_53F` with an optional upper half, per-source cycle end, and runtime 32-to-64 upgrade. Corrected 0.1-degree geometry is shared; 64-slot-only ESR fields use explicit aliases for 0.05 m/s² acceleration, lateral rate, width, oncoming, grouping, bridge-object, and range mode, while 32-slot acceleration retains 0.02 m/s². |
|
||||
| `0x4E0–0x4E5` | 8 B at the `500` radar cadence | Front-radar companion/status | Strong source association across every sampled `500` route. Shared `4E0` is decoded as ESR vehicle speed, yaw, curvature, 16-bit scan index, timestamp, counter, and communication health. The 64-slot radar's `4E1` matches ESR Status2; 32-slot `4E1` retains only the shared counter plus unresolved static metadata. `4E3` path IDs are shared and route-validated. The 32-slot-only `4E2`, `4E4`, and `4E5` carry BCD build metadata, protocol-derived raw ADC channels, and factory/vertical alignment status. |
|
||||
| `0x602–0x617` | 8 B, two packed tracks in the implemented messages | Front radar | Implemented legacy front layout. The relationship between `602–611` and the alternate `612–617` definitions remains incomplete. |
|
||||
| `0x210–0x21F` | 16 × 32 B at 20 Hz, two tracks per message | Front radar | Confirmed 32-track front layout. Detailed and compact dialects share the same core. |
|
||||
| `0x3A5–0x3C4` | 32 × 24 B at 20 Hz, one track per message | Front radar | Confirmed front layout. EV9 and Ioniq 9 populate its optional object-size/orientation extension. |
|
||||
| `0x235–0x248` | 20 × 32 B at about 33 Hz | **Forward-camera objects** | Corrected classification. Its first 124 bits exactly match the downloaded `FR_CMR_Obj` schema and carry camera quality, class, motion, ID, geometry, velocity, and acceleration. It is not corner radar. |
|
||||
| `0x240–0x24F` + `0x270–0x277` | status + 15 × 24 B objects + 8 × 32 B scan blocks | Candidate right-front corner channel A | Strong two-sensor topology on older HDA2 Ioniq 5, Palisade 2023, and EV6 routes. A=right is route/video-inferred; tracked-object position and velocity are decoded. Compact scan semantics remain unresolved. |
|
||||
| `0x278–0x28F` | status + 15 × 24 B objects + 8 × 32 B scan blocks | Candidate left-front corner channel B | Symmetric partner to channel A. B=left is route/video-inferred. The compact low field is only a distance candidate; property/auxiliary bits are unresolved. |
|
||||
| `0x3D0–0x3D4` | 5 × 32 B, seven packed records per message | Front-radar auxiliary scan list | Optional companion to `3A5–3C4` on sampled EV9/Ioniq 9 routes. It has 32 usable record positions and shares the radar cycle counter, but synchronized testing falsified the former radial-speed and azimuth interpretations. The low 12 bits remain only a distance candidate. |
|
||||
| `0x360–0x366` | 7 × 32 B | **Forward-camera lane/path** | Camera lane/path geometry, not radar. `362` contains lane confidence; `360/361/363/364` are active geometry, while `365/366` are often default-filled optional slots. |
|
||||
| legacy `0x238–0x24F` or `0x238–0x255` | 24 or 30 × 8 B at about 32 Hz | **Candidate forward-camera perception** | All 13 checked legacy routes carry one of these exact variants. Several carry a simultaneous `500` front radar at a different cadence, arguing strongly against this being a second radar. The 30-message variant also carries synchronized `25A–25E`; exact fields remain unparsed. Do not confuse these 8-byte messages with the CAN-FD `235–248` camera layout or `240/270` corner layout. |
|
||||
| CAN-FD `0x180–0x184`, `0x1B6–0x1B9`, `0x2BB–0x2BE` | 32 B at about 30 Hz | **Forward-camera auxiliary lists** | Downloaded Hyundai DBCs identify the transmitters/messages as camera traffic, and route payloads have camera-list cadence and slot behavior. Not a radar family. |
|
||||
| `CCNC_0x162` | fixed lead/left/right/rear slots | Fused/cluster output | Stock ADAS/cluster output with unknown upstream fusion. Do not publish it as independent raw radar. |
|
||||
| bus 9 `0x300/0x400/0x500/0x600` eight-address blocks | old prototype, eight subtracks per message | Four-corner prototype | Prior EV6 reverse-engineering mapped four blocks to FL/FR/RL/RR, but status and relative speed were unfinished and the family has not been reconfirmed in the current corpus. |
|
||||
|
||||
The confirmed `3A5–3C4` layout was checked across 21 compatible forum routes
|
||||
and 817,852 checksum-valid frames. All 726 advertised segments from the 20
|
||||
unique `210–21F` routes were scanned: 16 routes had active targets, three had
|
||||
a complete but empty stream, and one non-HDA2 Telluride route had no 210
|
||||
range. The 16 active routes split evenly between detailed and compact
|
||||
dialects, and all 13,210,543 checked 210 frames passed the HKG checksum.
|
||||
|
||||
The forum candidates are now classified at the family level. The legacy
|
||||
`238–24F` and `238–255` variants are strong forward-camera candidates but
|
||||
remain unparsed. The `180/1B6/2BB` groups are CAN-FD camera auxiliary lists.
|
||||
The incomplete Ioniq PHEV `500–?` observation is a complete `500–53F`
|
||||
64-slot front radar. None must be conflated with the CAN-FD `235` camera or
|
||||
`240/270` corner layouts merely because their addresses overlap.
|
||||
|
||||
The `4E0/4E1` 2-bit counters matched in all 2,552 paired frames checked across
|
||||
seven routes. On the three 64-slot routes, `4E3` matched the same counter in
|
||||
all 1,133 paired frames. Every one of the 2,369 distinct `4E0` 16-bit
|
||||
`SCAN_INDEX` steps incremented by one. In a fresh six-route full-segment pass,
|
||||
decoded `4E0` vehicle speed correlated with `carState.vEgo` at
|
||||
0.9976–0.99996, with 0.02–0.19 m/s mean absolute error. Both checked 64-slot
|
||||
routes held `MAXIMUM_TRACKS_ACK=64`; all four 32-slot routes decoded that ESR
|
||||
field as 1, confirming a different `4E1` dialect. The four sampled 32-slot
|
||||
variants carried plausible BCD build timestamps in `4E2`; their remaining
|
||||
`4E1` metadata stayed platform-static. All 7,347 nonzero `4E3` path IDs
|
||||
referenced active one-based target slots. The 32-slot `4E4/4E5` payloads match
|
||||
Hyundai-relocated ESR Status5/6 layouts: raw ADC health channels and alignment
|
||||
status. Across 4,698 `4E5` frames, both factory-alignment fields remained
|
||||
within the documented 0–3 enum subset; unexercised flags and physical ADC or
|
||||
misalignment scales remain research-only.
|
||||
|
||||
Two other consecutive lookalikes are currently excluded from radar work:
|
||||
downloaded Hyundai DBCs identify `630–63D` as head-unit/amplifier traffic, and
|
||||
`5ED–5EF` occurs on routes without `500` tracks and does not follow the
|
||||
otherwise strong `4E0`/`500` source association.
|
||||
|
||||
### Outstanding forum platform queue
|
||||
|
||||
| Platform | Remaining candidate |
|
||||
| --- | --- |
|
||||
| Hyundai Ioniq PHEV / Ioniq HEV 2022 / Palisade / Kia Niro PHEV 2022 | legacy camera candidate `238–24F`, plus its synchronized `201/20A/266/26D` companion groups |
|
||||
| Hyundai Custin / Elantra 2021 / Elantra HEV 2021 / Sonata / Sonata Hybrid | extended legacy camera candidate `238–255`, plus `25A–25E` and the same companion groups |
|
||||
| Kia Sorento / Sorento HEV 4th gen / Genesis GV70 | confirm semantics within the classified CAN-FD camera `180/1B6/2BB` auxiliary groups |
|
||||
| Legacy `500` front-radar platforms | identify static 32-slot `4E1` metadata and calibrate/actively exercise the decoded `4E4/4E5` health and alignment fields |
|
||||
|
||||
The forum also listed Acura MDX and Honda Clarity examples. They remain out of
|
||||
this Hyundai/Kia/Genesis inventory because the Acura range was unrelated and
|
||||
the Honda routes did not expose tracks.
|
||||
|
||||
## Confirmed and possible vehicle combinations
|
||||
|
||||
These are distinct source combinations supported by the corpus. “Possible”
|
||||
describes protocol combinations, not a promise that every trim exposes them.
|
||||
|
||||
| Combination | Front tracked objects | Corner source | Camera/auxiliary source | Known examples or status |
|
||||
| --- | --- | --- | --- | --- |
|
||||
| Legacy front only A | `500–51F` or `500–53F` | none observed | `4E0–4E5` radar companion/status | 32- and 64-slot platform variants |
|
||||
| Legacy front only B | `602–617` | none observed | none established | Kona EV 2022, Ceed PHEV candidates |
|
||||
| CAN-FD front only A | `210–21F` | none observed | none established | Elantra HEV 2024 and compact-dialect Tucson/Santa Cruz/Sportage routes |
|
||||
| CAN-FD front + paired front corners | `210–21F` | inferred right A `240–24F` + `270–277`; inferred left B `278–28F` | none required | Older HDA2 Ioniq 5, Palisade 2023, and EV6 observations |
|
||||
| CAN-FD front only B | `3A5–3C4` | none observed | camera objects/lanes may be absent or not logged | Compatible HDA1 routes |
|
||||
| CAN-FD front + camera perception | `3A5–3C4` | none established | camera objects `235–248` and lanes `360–366` | Common newer HDA2 pattern; camera traffic is not another radar |
|
||||
| Rich front + camera perception + auxiliary radar scan | rich `3A5–3C4` | none established | `235–248`, `360–366`, plus radar auxiliary records `3D0–3D4` | EV9 and Ioniq 9 sampled routes |
|
||||
| Front + four-corner prototype | layout depends on the old EV6 capture | bus-9 FL/FR/RL/RR blocks | unknown | Historical prototype only; not current-production support |
|
||||
|
||||
No current evidence proves a combination containing both the paired
|
||||
`240/270` corner family and the newer `3D0` auxiliary scan family. No distinct
|
||||
rear-radar tracked-object range has been confirmed in the current routes.
|
||||
Rear objects shown by `CCNC_0x162` may be fused from other sensors and do not
|
||||
prove a rear radar source.
|
||||
|
||||
## Platform observations
|
||||
|
||||
| Platform group | Front | Additional observed traffic |
|
||||
| --- | --- | --- |
|
||||
| Ioniq 5 / Palisade 2023 / EV6 older HDA2 samples | detailed `210–21F` | paired `240/270–28F` front corners; A=right and B=left strongly inferred |
|
||||
| Elantra HEV 2024 | detailed `210–21F` | no corner family established |
|
||||
| Santa Cruz 2025 / Tucson family / Sportage 5th gen | compact `210–21F` | no corner family established in the checked samples |
|
||||
| Ioniq 6 / Kona 2 / Kona EV 2 / Santa Fe 5 / Sonata 2024 / Carnival HEV / EV6 2025 / K4 / K5 / Niro 2 | common `3A5–3C4` where compatible | platform-dependent camera `235` and `360` traffic; no decoded corner family established |
|
||||
| EV9 / Ioniq 9 | rich `3A5–3C4` | camera objects `235`, camera lanes `360`, optional front-radar auxiliary scan `3D0` |
|
||||
| Sportage HEV 2026 forum sample | overlapping but incompatible `3A5` bytes | excluded from the common-layout decoder |
|
||||
|
||||
HDA2 changes where traffic is exposed, but raw bus number is not a reliable
|
||||
layout discriminator. Follow sustained logical A-CAN and reject short
|
||||
forwarded copies.
|
||||
|
||||
## What remains to prove
|
||||
|
||||
1. Confirm the route/video-derived A=right and B=left assignment with a
|
||||
controlled sensor occlusion or authoritative service mapping.
|
||||
2. Finish both corner status messages and the remaining 24-byte object
|
||||
fields: the stable 2-bit unknown category, persistent ID, classification,
|
||||
dimensions, and sensor-health flags. The former acceleration candidate was
|
||||
falsified and returned to raw bits. Position and longitudinal/lateral
|
||||
velocity are resolved, and `0x278` byte 3 is the confirmed pre-truncation
|
||||
channel-A object count: exported A slots equal `min(count, 15)`.
|
||||
Preserve all remaining unknown bits until they correlate with the compact
|
||||
scan bins.
|
||||
3. Decode the 56 fixed-position `270–277` / `288–28F` scan candidates,
|
||||
including the low 13-bit distance candidate, seven-bit property, and
|
||||
nine-bit auxiliary fields. Direct same-cycle range association and a linear
|
||||
record-index-to-angle map both failed time-shift controls, so do not project
|
||||
these records as geometry until an independent mapping is found.
|
||||
4. Decode the `3D0` auxiliary scan array without restoring the falsified
|
||||
radial-speed or azimuth names. Determine whether the low 12 bits are
|
||||
distance, identify the role and physical angular ordering of the 32 record
|
||||
positions, and decode the four flag bits plus both unknown bytes. Two rich
|
||||
routes confirm exact counter synchronization and the sentinel/zero empty
|
||||
forms, but reject range-only, candidate-geometry, and direct slot-to-slot
|
||||
associations with `3A5`. Production RadarData must continue to exclude it.
|
||||
5. Decode the active polynomial/path fields in `360/361/363/364`. Treat
|
||||
`365/366` constant payloads as unavailable lane slots, not missing radar.
|
||||
6. Resolve `235–248` `UNKNOWN_1/2/3`, identify every checksum variant, and
|
||||
validate native camera classification/motion values against video. Confirm
|
||||
the camera-source assignment and forwarding path on each platform carrying
|
||||
the range.
|
||||
7. Search explicitly for the old four-corner topology and for a distinct rear
|
||||
tracked-object source. Record verified absence when the correct buses and
|
||||
maneuvers were covered.
|
||||
8. Resolve the static 32-slot `4E1` metadata, physically calibrate the
|
||||
protocol-derived `4E4` ADC channels, and capture alignment events to verify
|
||||
the inactive `4E5` flags and misalignment scales. Exercise the currently
|
||||
zero FCW path IDs and low `4E3` status flags on a 32-slot platform. Shared
|
||||
`4E0` and path IDs are resolved; do not transfer unsupported 64-slot
|
||||
Status2 fields onto the 32-slot payload.
|
||||
9. Decode the legacy camera candidates `238–24F` and `238–255`, including
|
||||
their `201/20A/25A/266/26D` companion groups. Determine whether `250–255`
|
||||
and `25A–25E` are extra objects, path/lane data, or metadata, and correlate
|
||||
classification and geometry against video. Keep these out of production
|
||||
RadarData unless source evidence overturns the current camera assignment.
|
||||
10. Complete the per-platform combination matrix across HDA1/HDA2 and
|
||||
CCNC/non-CCNC cars. Record the sustained logical CAN source, forwarded
|
||||
echoes, empty-but-present ranges, and verified absences rather than relying
|
||||
on raw bus numbers.
|
||||
11. Add representative replay fixtures and debugger/on-road UI tests for each
|
||||
confirmed source combination. Promote candidate names or fields only after
|
||||
multi-route and controlled-scene validation.
|
||||
|
||||
## Useful work that does not require video
|
||||
|
||||
1. Cluster every unknown field by platform, object lifecycle, motion class,
|
||||
address/slot, and empty/default state. Stable cross-platform enums can be
|
||||
separated from platform configuration bytes.
|
||||
2. Use consecutive-cycle derivatives and ego-motion compensation to test
|
||||
velocity, acceleration, yaw, and sensor-frame hypotheses. Always compare
|
||||
against opposite-channel and time-shifted controls.
|
||||
3. Search counter phase and bounded latency, not merely nearest timestamps,
|
||||
when testing associations between tracked objects, compact scan arrays,
|
||||
camera objects, and status messages.
|
||||
4. Build transition matrices for the corner unknown category and remaining
|
||||
flags. The first pass proved the 2-bit category is
|
||||
stable within a continuous object but is not lifecycle or validity: all four
|
||||
values occur on active targets, while empty slots use `LONG_DIST=204.7 m`.
|
||||
Continue searching the remaining fields for birth, coast, replacement, and
|
||||
deletion behavior without assuming the visual object class.
|
||||
5. Compare the same physical field across detailed `210`, compact `210`,
|
||||
`3A5`, corner objects, and legacy `500` tracks after normalizing coordinates
|
||||
and lifecycle. Shared scaling should survive route and platform changes.
|
||||
6. Mine downloaded and public DBC/source comments for transmitter identity and
|
||||
bit boundaries, but require route evidence before adopting physical names or
|
||||
scales.
|
||||
7. Measure source dropouts, checksum/counter discontinuities, and status-bit
|
||||
transitions together. This can identify communication health, blockage,
|
||||
alignment, and reset fields without video.
|
||||
8. Generate debugger capture instructions for later physical confirmation:
|
||||
cover one corner sensor at a time, use a single stationary target at a
|
||||
measured distance, and record straight approach/recede passes at known
|
||||
speed. These tests can settle side assignment and physical scan fields with
|
||||
much less ambiguity than unconstrained road scenes.
|
||||
|
||||
## Implementation policy
|
||||
|
||||
- Production RadarData contains the confirmed tracked-object layouts only.
|
||||
- `235–248` remains available through RadarData as a camera-object source for
|
||||
research, with explicit source labeling and its native object-ID validity.
|
||||
- The corner and `3D0` generators are research DBCs. Their unresolved fields
|
||||
are named unknown or candidate in comments and are not used for control.
|
||||
- `4E0–4E5` status fields are present in the `RADAR_500_53F` DBC for research,
|
||||
and inventoried by the desktop radar debugger, but are not required for
|
||||
source detection or production track parsing.
|
||||
- The on-road UI labels `235–248` as `CAM` and uses triangle markers.
|
||||
- The desktop radar debugger plots confirmed production tracks, the
|
||||
debugger-only `612–617` alternate objects, and the decoded `240/278` corner
|
||||
objects. Its `SIGNALS` table exposes the compact `270/288` records, `3D0`
|
||||
records, `4E0–4E5` status, `360–366` raw camera payloads, and all six
|
||||
`CCNC_0x162` fused slots. Compact scans and `3D0` remain table-only and are
|
||||
never projected as geometry. `0x162` is explicitly labeled fused and is not
|
||||
published as an independent radar source.
|
||||
@@ -0,0 +1,450 @@
|
||||
# Hyundai radar, camera-object, and auxiliary signal notes
|
||||
|
||||
This document tracks the evidence behind the 24-byte `RADAR_3A5_3C4` layout
|
||||
and its shared signal semantics with the 32-byte `RADAR_210_21F` layout.
|
||||
It also records the now-identified `0x235–0x248` camera objects and the
|
||||
research-only corner and raw-detection families. It deliberately separates a
|
||||
signal being active from its meaning being known.
|
||||
The cross-family and per-platform investigation plan lives in
|
||||
[RADAR_RESEARCH.md](RADAR_RESEARCH.md).
|
||||
The forum-ready platform/range summary lives in
|
||||
[PLATFORM_MATRIX.md](PLATFORM_MATRIX.md).
|
||||
|
||||
## Status terms
|
||||
|
||||
- **Confirmed core**: populated across compatible platforms and its meaning is
|
||||
supported by position, motion, or lifecycle behavior.
|
||||
- **Optional useful**: meaning is supported, but many platforms transmit a
|
||||
default value instead of the signal.
|
||||
- **Active unknown**: the field changes consistently, but its meaning is not
|
||||
established well enough to use.
|
||||
- **Redundant**: understood, but duplicates a better primary field.
|
||||
- **Reserved/dead**: remained zero across the tested raw frames.
|
||||
|
||||
An optional field that is dead on one platform is not globally dead.
|
||||
|
||||
## Evidence corpus
|
||||
|
||||
- The original layout was checked against 817,852 frames from 21 forum routes.
|
||||
- The 3A5 checksum was rechecked on all 817,852 frames and matched the HKG
|
||||
CAN-FD checksum with the CAN address as data ID, with zero failures.
|
||||
- All 726 advertised 210-route segments were scanned across 20 unique routes:
|
||||
16 routes had active targets, three had complete but empty streams, and one
|
||||
non-HDA2 Telluride route had no 210 stream. The 16 active routes split evenly
|
||||
between detailed and compact dialects, with no third active layout.
|
||||
- The 210 HKG checksum matched all 13,210,543 checked frames.
|
||||
- The documented route corpus was expanded to 107 route/segment entries:
|
||||
all 36 canonical Hyundai/Kia CAN-FD test routes plus the forum radar and
|
||||
consecutive-range route lists. A qlog pre-screen identified candidates
|
||||
carrying 24-byte 3A5 traffic, then full rlogs were used for layout decisions.
|
||||
- The combined full-rlog pass contains 24 compatible route segments and
|
||||
492,679 active (`STATE` 3/4) observations. Fourteen segments are HDA2 and ten
|
||||
are HDA1. The compatible set covers every architecture combination:
|
||||
HDA1/HDA2 and CCNC/non-CCNC.
|
||||
- Full geometry/kinematics data was populated only by EV9 and Ioniq 9. Both
|
||||
are HDA2 + CCNC, but HDA2 + CCNC Kona EV, EV6 2025, and K4 routes left those
|
||||
fields at their unavailable defaults. A second Ioniq 9 segment independently
|
||||
reproduced the rich extension.
|
||||
- The 36 canonical CAN-FD test routes contributed eight compatible 3A5
|
||||
segments and 150,489 active observations. This independently reproduced the
|
||||
layout on HDA1 and HDA2 cars.
|
||||
- An additional eight-segment local recording supplied 89,233 active track
|
||||
observations. Its replay fingerprint is unavailable, so it is used for
|
||||
layout/range evidence but not platform attribution.
|
||||
- The known reserved bits were rechecked across 320,454 additional K5 and
|
||||
local-recording frames; all remained zero.
|
||||
- The Sportage HEV 2026 forum route has an incompatible overlapping layout and
|
||||
is excluded from this matrix.
|
||||
- A downloaded Hyundai DBC archive supplied two related but non-bit-compatible
|
||||
object layouts. The BN7 front-radar DBC defines a persistent global object
|
||||
ID and `0/1/2/3 = unknown/stationary/moving/stopped`; the HMVS4 object DBC
|
||||
independently defines value 3 as stopped, a 7-bit quality level, and alive
|
||||
age/lifetime. Neither DBC directly contains the 210 or 3A5 radar layout.
|
||||
- The same archive's `FR_CMR_Obj` definition matches the first 124 bits of
|
||||
`0x235–0x248` exactly. Together with its 33 Hz cadence and co-location with
|
||||
`0x360–0x366`, this corrects the former corner-radar classification:
|
||||
`0x235–0x248` is a forward-camera object list.
|
||||
- The repository's Delphi `ESR.dbc` exactly matches the `0x500–0x53F` target
|
||||
packing and the 64-slot radar's `0x4E0/0x4E1/0x4E3` status packing. A fresh
|
||||
six-route full-segment check covered two 64-slot and four 32-slot variants.
|
||||
`0x4E0` radar speed correlated with `carState.vEgo` at 0.9976–0.99996 with
|
||||
0.02–0.19 m/s mean absolute error. Both 64-slot variants acknowledged 64
|
||||
tracks in `0x4E1`; the four 32-slot payloads decoded as 1, proving that
|
||||
their `0x4E1` uses a different dialect.
|
||||
|
||||
## 500-53F legacy front-radar layout
|
||||
|
||||
Every eight-byte message is one target slot. The lower 32 slots
|
||||
`0x500–0x51F` are required; some radar variants add an identically packed
|
||||
upper bank at `0x520–0x53F`. Inter-arrival timing on all six freshly checked
|
||||
full segments is about 50 ms, confirming the established 20 Hz cadence. A gap
|
||||
inside these logs makes frame count divided by total segment span misleading.
|
||||
|
||||
| Signal | Start bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `UNKNOWN_1` | 7 | 8 signed | raw | Preserve on 32-slot variants. Its low bits are active but do not behave like ESR target flags. |
|
||||
| `ONCOMING_ESR` | 0 | 1 | boolean | 64-slot only. Every flagged target in both full-segment checks had negative ground-frame speed. Clear does not distinguish stationary from same-direction moving. |
|
||||
| `GROUPING_CHANGED_ESR` | 1 | 1 | boolean | 64-slot ESR identity; exact tracker behavior remains to be correlated. |
|
||||
| `REL_LAT_SPEED_ESR` | 7 | 6 signed | 0.25 / 0 m/s | 64-slot ESR lateral target rate. It is not published for 32-slot variants. |
|
||||
| `AZIMUTH` | 12 | 10 signed | 0.1 / 0 deg | Corrected from the former 0.2-degree scale. Lateral projection now uses the angle directly rather than a compensating half-scale. |
|
||||
| `STATE` | 15 | 3 | enum | Track lifecycle; values 3 and 4 are the measured/coasted valid states used by the parser. |
|
||||
| `LONG_DIST` | 18 | 11 | 0.1 / 0 m | Target range. |
|
||||
| `REL_ACCEL` | 33 | 10 signed | 0.02 / 0 m/s² | 32-slot dialect scale retained from route behavior. |
|
||||
| `REL_ACCEL_ESR` | 33 | 10 signed | 0.05 / 0 m/s² | 64-slot ESR scale. Separate overlapping names prevent applying it to the 32-slot dialect. |
|
||||
| `WIDTH_ESR` | 37 | 4 | 0.5 / 0 m | 64-slot ESR target width. It was populated on both checked 64-slot routes and identically zero on all four 32-slot routes. |
|
||||
| `COUNTER` | 38 | 1 | 1 / 0 | Per-target rolling count. |
|
||||
| `BRIDGE_OBJECT_ESR` | 39 | 1 | boolean | 64-slot ESR identity; exact tracker behavior remains to be correlated. |
|
||||
| `REL_SPEED` | 53 | 14 signed | 0.01 / 0 m/s | Radial range rate. |
|
||||
| `MED_RANGE_MODE_ESR` | 55 | 2 | enum | 64-slot ESR medium-range operating-mode field. |
|
||||
|
||||
### 4E0-4E5 companion messages
|
||||
|
||||
`0x4E0` has the shared ESR status layout on both 32- and 64-slot variants:
|
||||
|
||||
| Signal | Start bit | Size | Scale / offset |
|
||||
| --- | ---: | ---: | --- |
|
||||
| `DSP_TIMESTAMP` | 5 | 7 | 2 / 0 ms |
|
||||
| `GROUP_COUNTER` | 6 | 2 | 1 / 0 |
|
||||
| `COMM_ERROR` | 14 | 1 | boolean |
|
||||
| `RADIUS_CURVATURE` | 13 | 14 signed | 1 / 0 m |
|
||||
| `SCAN_INDEX` | 31 | 16 | 1 / 0 |
|
||||
| `YAW_RATE` | 47 | 12 signed | 0.0625 / 0 deg/s |
|
||||
| `VEHICLE_SPEED` | 50 | 11 | 0.0625 / 0 m/s |
|
||||
|
||||
The 64-slot variant exactly matches ESR Status2 at `0x4E1`. On 32-slot
|
||||
variants only its low two-bit rolling counter is active; the other 62 bits
|
||||
are platform-static and do not acknowledge 32 tracks, so the ESR Status2
|
||||
names remain 64-slot-only. The `0x4E3` in-path path-ID bytes are shared across
|
||||
both variants, while its low status flags, counter, range mode, and alignment
|
||||
angle are confirmed only on the 64-slot variant.
|
||||
|
||||
| `0x4E1` 64-slot signal | Start bit | Size | Scale / offset |
|
||||
| --- | ---: | ---: | --- |
|
||||
| `GROUP_COUNTER` | 1 | 2 | 1 / 0 |
|
||||
| `MAXIMUM_TRACKS_ACK` | 7 | 6 | 1 / 1 |
|
||||
| `STEERING_ANGLE_ACK` | 10 | 11 | 1 / 0 deg |
|
||||
| `RAW_DATA_MODE`, `TRANSCEIVER_OPERATIONAL`, `INTERNAL_ERROR`, `RANGE_PERFORMANCE_ERROR`, `OVERHEAT_ERROR` | 11–15 | 1 each | boolean |
|
||||
| `TEMPERATURE` | 31 | 8 signed | 1 / 0 °C |
|
||||
| `GROUPING_MODE` | 33 | 2 | 1 / 0 |
|
||||
| `VEHICLE_SPEED_COMP_FACTOR` | 39 | 6 signed | 0.00195 / 1 |
|
||||
| `YAW_RATE_BIAS` | 47 | 8 signed | 0.125 / 0 deg/s |
|
||||
| `DSP_SOFTWARE_VERSION` | 55 | 16 | 1 / 0 |
|
||||
|
||||
| `0x4E3` signal | Start bit | Size | Scale / offset |
|
||||
| --- | ---: | ---: | --- |
|
||||
| `GROUP_COUNTER` | 1 | 2 | 1 / 0; 64-slot confirmed |
|
||||
| `MEDIUM_LONG_RANGE_MODE` | 3 | 2 | 1 / 0; 64-slot confirmed |
|
||||
| `PARTIAL_BLOCKAGE`, `SIDELOBE_BLOCKAGE`, `LONG_RANGE_GRATING_LOBE_DETECTED`, `TRUCK_TARGET_DETECTED` | 4–7 | 1 each; 64-slot confirmed |
|
||||
| `ACC_MOVING_PATH_ID` | 15 | 8 | one-based target slot; shared |
|
||||
| `CMBB_MOVING_PATH_ID`, `CMBB_STATIONARY_PATH_ID` | 23, 31 | 8 each | one-based target slots; shared |
|
||||
| `FCW_MOVING_PATH_ID`, `FCW_STATIONARY_PATH_ID` | 39, 47 | 8 each | protocol-defined; unexercised on sampled 32-slot routes |
|
||||
| `AUTO_ALIGN_ANGLE` | 55 | 8 signed | 0.0625 / 0 deg; 64-slot confirmed |
|
||||
| `ACC_STATIONARY_PATH_ID` | 63 | 8 | one-based target slot; shared |
|
||||
|
||||
All 7,347 nonzero `0x4E3` path-ID observations across the four 32-slot routes
|
||||
referenced an active `0x500–0x51F` target slot. IDs reached 32, proving
|
||||
one-based indexing. ACC and CMBB moving selectors commonly referenced the
|
||||
same slot, as did their stationary selectors.
|
||||
|
||||
The 32-slot variants additionally send `0x4E2`, `0x4E4`, and `0x4E5`:
|
||||
|
||||
- `0x4E2` consistently encodes plausible BCD build timestamps across all four
|
||||
sampled platforms.
|
||||
- `0x4E4` matches the byte order and address-shifted envelope of Delphi ESR
|
||||
Status5: switched-battery, ignition, two temperature, and four supply ADC
|
||||
channels. The identities are protocol-derived and the raw ADC values are
|
||||
intentionally uncalibrated; they must not drive a health decision.
|
||||
- `0x4E5` matches the similarly relocated ESR Status6 alignment payload. In
|
||||
4,698 checked frames its two factory-alignment fields used only documented
|
||||
enum values 0–3. The other flags and alignment-value bytes remained zero,
|
||||
so their identities are protocol-derived but not independently exercised.
|
||||
|
||||
Although generic ESR defines eight-byte input messages at `0x4F0/0x4F1`, no
|
||||
such messages were present on these source buses. The observed Hyundai
|
||||
`0x4F1` was four bytes on other/forwarded buses and is not assigned the ESR
|
||||
input schema.
|
||||
|
||||
## Signal matrix
|
||||
|
||||
| Signal | Status | Availability and evidence |
|
||||
| --- | --- | --- |
|
||||
| `CHECKSUM` | Confirmed core | 16-bit HKG CAN-FD checksum using the CAN address as data ID. It matched all 817,852 checked 3A5 frames. |
|
||||
| `COUNTER` | Confirmed core | Global transmit-cycle counter covering 0-255 and wrapping. |
|
||||
| `STATE_ALT` | Redundant | Exact compressed mirror in all 325,110 active samples: `STATE=3 -> 2`, `STATE=4 -> 3`. |
|
||||
| `MOTION_STATE` | Confirmed core | Ground-frame class: unknown, stationary, or moving. Correctly marked observed crossing traffic as moving. Related Hyundai radar data assigns the unused fourth 2-bit value to stopped, not oncoming; that bit remained zero throughout the tested 3A5 corpus. |
|
||||
| `TRACK_COUNTER` | Confirmed core | Modulo-4 per-track update counter. Separate from the transmit counter. |
|
||||
| `TRACK_QUALITY` | Confirmed core | Unsigned 7-bit track quality/existence score. Empty and tentative tracks are low, measured tracks are high, and coasted tracks decline with `COAST_AGE`. A related Hyundai object layout describes its 7-bit quality level as reliability, validity, or probability evidence; the radar's raw scale is not confirmed to be a percentage. |
|
||||
| `AGE` | Confirmed core | Per-track alive age/lifetime count, 0-255. |
|
||||
| `COAST_AGE` | Confirmed core | Zero for measured tracks and increments while a track is coasted/predicted. |
|
||||
| `STATE` | Confirmed core | Track lifecycle: empty, tentative, measured, coasted, or unresolved tentative. |
|
||||
| `RCS` | Confirmed core | Signed radar cross-section/return-strength value. At controlled distances its median consistently increases from small to passenger to large targets. The raw unit is not calibrated. |
|
||||
| `LONG_DIST` | Confirmed core | Longitudinal target distance in meters. |
|
||||
| `LAT_DIST` | Confirmed core | Lateral target position relative to the ego axis in meters. |
|
||||
| `REL_SPEED` | Confirmed core | Longitudinal target speed relative to ego. |
|
||||
| `NEW_SIGNAL_4` | Active unknown | Category 0-2. Nonzero in about 25% of moving samples, 10% of stationary samples, and 6% of unknown samples; it is related to target state but is not another motion class. |
|
||||
| `REL_LAT_SPEED` | Confirmed core | Lateral target speed relative to the ego axis. A live crossing target showed `+6.78 m/s` lateral versus `-0.51 m/s` forward. An EV9 route independently supplied hundreds of perpendicular-motion samples. |
|
||||
| `REL_ACCEL` | Confirmed core | Longitudinal target acceleration relative to ego. |
|
||||
| `NEW_SIGNAL_18` | Optional active unknown | A lifecycle/measurement-status attribute on rich layouts. In the public EV9/Ioniq 9 data, values 1/2 occur almost exclusively while `STATE=3` is measured and 0 while `STATE=4` is coasted; one anomalous coasted sample retained 2. The distinction between 1 and 2 is unknown. Most platforms always send 0, and a compatible long recording contains the rare value 3. |
|
||||
| `NEW_SIGNAL_5` | Optional active unknown | Sparse value observed from 0 to 32 on both HDA1 and HDA2 routes, including platforms without rich geometry. No stable correlation is known. |
|
||||
| `WIDTH` | Optional useful | Target width in 0.1 m units. Populated on EV9/Ioniq 9; passenger-car values cluster around 1.8-2.0 m. Zero means unavailable on other sampled platforms. Every address in 3A5-3C4 can carry it, so it does not identify a separate track bank. |
|
||||
| `LENGTH` | Optional useful | Target length in 0.1 m units. Passenger cars cluster around 4 m and long vehicles reach roughly 13-15.5 m. It is available on the same target samples as `WIDTH`. |
|
||||
| `ABS_SPEED` | Optional useful | Absolute target-speed magnitude. On Ioniq 9 it matches `hypot(vEgo + REL_SPEED, REL_LAT_SPEED)` with about 0.08-0.15 m/s median error. It was populated for 28,735/28,860 active Ioniq 9 samples and 23,252/27,339 EV9 samples, including many targets without dimensions. Zero is the platform default when unsupported. |
|
||||
| `ORIENTATION_ANGLE` | Optional useful | Target orientation relative to the ego axis. Ioniq 9 follows motion heading within roughly 2 degrees median; EV9 crossing traffic rotated from about -45 to -83 degrees. It is associated with dimension-bearing targets. Values near `+/-180` can represent real rearward orientation on rich tracks, while compatible non-rich platforms consistently use +180 as unavailable/default. |
|
||||
| `NEW_SIGNAL_13` | Optional active unknown | Shape/geometry metadata from 0-10. It was nonzero only on dimension-bearing targets in the EV9/Ioniq 9 routes. Confidence-like behavior is plausible but not established. |
|
||||
| `NEW_SIGNAL_12` | Optional active unknown | Strong geometry-age/maturity candidate. Two independent Ioniq 9 routes progressed `0 -> 1 -> 2 -> ... -> 10` and saturated at 10. It remains unnamed pending confirmation on another rich platform. |
|
||||
| `NEW_SIGNAL_14` | Optional active unknown | Shape/geometry category 0-3. It is zero when the associated geometry metadata is unavailable. |
|
||||
| `NEW_SIGNAL_15` | Optional active unknown | Shape/geometry category 0-2. It is zero when the associated geometry metadata is unavailable. |
|
||||
| `NEW_SIGNAL_16` | Optional active unknown | Shape/geometry category 0-3. It is zero when the associated geometry metadata is unavailable. |
|
||||
| `NEW_SIGNAL_17` | Optional active unknown | Shape/geometry category 0-2. It is zero when the associated geometry metadata is unavailable. The most common rich-layout `14/15/16/17` tuple is `2/2/2/1`. |
|
||||
|
||||
## Cross-family layout and naming findings
|
||||
|
||||
Both families expose 32 tracks at approximately 20 Hz per address:
|
||||
|
||||
| Layout | Envelope | Tracks per message | Shared core | Family-specific extension |
|
||||
| --- | --- | --- | --- | --- |
|
||||
| 210 detailed | `0x210-0x21F`, 32 bytes | 2 | State, motion, quality, age/coast age, RCS, range, relative velocity/acceleration | Detailed state/RCS/category are populated; compact object-ID bytes are normally zero |
|
||||
| 210 compact | `0x210-0x21F`, 32 bytes | 2 | Same bit positions and scales except for the documented lateral-speed sign convention | Uses `STATE_ALT`, per-target status and persistent 6-bit `OBJECT_ID`; detailed state/RCS/category remain zero |
|
||||
| 3A5 common | `0x3A5-0x3C4`, 24 bytes | 1 | Homologous first 16 bytes | Bytes 16-23 carry measurement status and optional dimensions, absolute speed, orientation, and geometry metadata |
|
||||
|
||||
The shared signal conclusions are:
|
||||
|
||||
- The old signed `NEW_SIGNAL_2` decoding was wrong. Raw values use 0-99 in the
|
||||
tested corpus, and the signed interpretation created an artificial wrap at
|
||||
63/64. The standardized name is `TRACK_QUALITY`, unsigned 7-bit.
|
||||
- Detailed 210 and 3A5 `TRACK_QUALITY` distributions are nearly identical.
|
||||
Compact 210 uses a higher calibration, commonly around 70 while measured,
|
||||
that falls toward the low 20s at long coast ages. The semantic name is
|
||||
shared, but no percentage unit is assigned.
|
||||
- The old `NEW_SIGNAL_8` is standardized as `RCS`. Detailed 210 and 3A5 have
|
||||
closely matching distributions, and size ordering is preserved within every
|
||||
tested distance band. 210 supplies eight signed bits; 3A5 supplies seven
|
||||
because the remaining bit extends `LONG_DIST` from 12 to 13 bits.
|
||||
- `NEW_SIGNAL_4` occupies the same physical two bits and has nearly identical
|
||||
0-2 distributions in detailed 210 and 3A5, but its exact category remains
|
||||
unknown.
|
||||
- Compact 210 `OBJECT_ID` remains strongly supported: it held through 99.9%
|
||||
of consecutive same-track updates and changed when a slot acquired a
|
||||
replacement target. A related Hyundai BN7 radar DBC independently defines
|
||||
`ObjectId` as a global ID that remains constant while an object is tracked.
|
||||
Sparse 3A5 `NEW_SIGNAL_5` does not behave like the same field and remains
|
||||
unknown.
|
||||
|
||||
## Oncoming classification
|
||||
|
||||
Oncoming candidates were cross-checked using fresh ego speed and
|
||||
`vEgo + REL_SPEED < -3 m/s`.
|
||||
|
||||
| Layout | Evidence | Conclusion |
|
||||
| --- | --- | --- |
|
||||
| 210 compact | Motion code 4 appeared on 4,870 active rows. Synchronized code-4 rows had -13.30 m/s median ground-frame longitudinal speed, with 3,516 strong oncoming candidates. Related Hyundai BN7 radar and HMVS4 object DBCs independently label motion value 3 as stopped. | `MOTION_STATE=3` is stopped and `MOTION_STATE=4` is the compact oncoming class. |
|
||||
| 210 detailed | Bit 29 never set across 317,965 active rows. Its 4,924 physics-based candidates remained in motion values 0-2. | No dedicated detailed-210 oncoming value. |
|
||||
| 3A5 common | Bit 29 remained zero in all 817,852 frames. Among 6,104 physics-based candidates across 14 routes, motion was 0 for 5,792, 1 for 312, and never 2. | No dedicated 3A5 oncoming value or flag. |
|
||||
|
||||
`NEW_SIGNAL_4` and `NEW_SIGNAL_18` also failed to isolate the 3A5 oncoming
|
||||
candidates. Do not widen 3A5 `MOTION_STATE` to three bits. If oncoming
|
||||
classification is needed for 3A5 or detailed 210, derive it from ego-relative
|
||||
kinematics with persistence, hysteresis, and lane plausibility.
|
||||
|
||||
## Platform and cluster conclusions
|
||||
|
||||
| Architecture | Compatible examples | Rich geometry |
|
||||
| --- | --- | --- |
|
||||
| HDA2 + CCNC | Kona EV 2, EV6 2025, K4, EV9, Ioniq 9 | EV9 and Ioniq 9 only |
|
||||
| HDA2 + non-CCNC | Ioniq 6, Niro EV 2, Carnival HEV | None observed |
|
||||
| HDA1 + CCNC | Kona 2/HEV, Sonata/HEV, K4, K5, Santa Fe 5 | None observed |
|
||||
| HDA1 + non-CCNC | Niro EV 2/HEV | None observed |
|
||||
|
||||
- HDA2 changes the normal 3A5 bus from bus 1 to bus 0, but does not imply
|
||||
rich extension support.
|
||||
- EV6 2025 carried a complete 3A5 range on buses 0 and 2. The bus-2 subset was
|
||||
a 99.6% exact byte-for-byte subset of bus 0, consistent with forwarding
|
||||
rather than a second radar. The runtime selects one active bus per range.
|
||||
- The optional bytes are attributes on the same 32 addresses, not extra
|
||||
objects. Dimension-bearing targets appeared throughout 3A5-3C4 rather than
|
||||
in a dedicated address subset.
|
||||
- Stock `CCNC_0x162` is a separate fused cluster-object output. Two HDA2 +
|
||||
CCNC Kona EV routes exercised lead, left/right, and both rear slots while
|
||||
every 3A5 width/length/absolute-speed/shape field remained unavailable.
|
||||
Rear-slot distance capped at 20 m and the stock lateral value was fixed at
|
||||
3.0 m. `LEAD_ALT` remained hidden in both sampled segments.
|
||||
- Therefore, the additional cluster tofus are real stock CCNC/ADAS outputs,
|
||||
but they cannot be reconstructed merely by reading the 3A5 rich extension.
|
||||
Rear tofus in particular need a source beyond the forward 3A5 radar.
|
||||
|
||||
## 235-248 forward-camera object layout
|
||||
|
||||
Each 32-byte message is one camera-object slot. `OBJECT_ID=0` is unused in the
|
||||
observed routes. The production parser normalizes the native camera motion
|
||||
enum to the shared `unknown/stationary/moving` display enum, but preserves the
|
||||
raw DBC value.
|
||||
|
||||
| Signal | Start bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `CHECKSUM` | 0 | 16 | 1 / 0 | Checksum field; platform variants exist, so the generator does not force the standard HKG algorithm. |
|
||||
| `COUNTER` | 16 | 8 | 1 / 0 | Transmit-cycle counter. |
|
||||
| `QUALITY` | 24 | 7 | 1 / 0 | Camera-object quality/reliability level. |
|
||||
| `AGE` | 32 | 8 | 1 / 0 | Object alive age. |
|
||||
| `MOTION_STATE` | 40 | 4 | 1 / 0 | Native camera motion class. |
|
||||
| `OBJECT_ID` | 44 | 7 | 1 / 0 | Persistent object ID; zero is unused. |
|
||||
| `WIDTH` | 52 | 7 | 0.05 / 0 m | Estimated width. |
|
||||
| `CLASSIFICATION` | 60 | 3 | 1 / 0 | Unknown, truck, car, motorcycle, bicycle, pedestrian, undecided. |
|
||||
| `LONG_DIST` | 64 | 13 | 0.05 / 0 m | Longitudinal relative position. |
|
||||
| `LAT_DIST` | 78 | 12 | 0.05 / -102.4 m | Lateral relative position. |
|
||||
| `REL_SPEED` | 91 | 12 | 0.05 / -100 m/s | Longitudinal relative velocity. |
|
||||
| `REL_LAT_SPEED` | 104 | 10 | 0.05 / -25 m/s | Lateral relative velocity. |
|
||||
| `REL_ACCEL` | 115 | 9 signed | 0.05 / 0 m/s² | Longitudinal relative acceleration. |
|
||||
| `UNKNOWN_1` | 125 | 12 | raw | Active extension; semantics unknown. |
|
||||
| `UNKNOWN_2` | 138 | 12 | raw | Active extension; semantics unknown. |
|
||||
| `UNKNOWN_3` | 151 | 10 | raw | Active extension; semantics unknown. |
|
||||
| `AZIMUTH` | 176 | 14 | 360/16384 / -180 deg | Route-validated high-resolution object azimuth. |
|
||||
|
||||
Native motion values are: 0 undefined, 1 standing, 2 parked, 3 stopped,
|
||||
4 unknown movable, 5 moving, 6 stopped oncoming, 7 unknown oncoming,
|
||||
8 moving oncoming, and 9 crossing bicycle. This is the only newly decoded
|
||||
family with explicit oncoming moving and stopped-oncoming values.
|
||||
|
||||
## 240/270-28F candidate corner-radar layout
|
||||
|
||||
The address sizes and symmetry form two complete sensor channels:
|
||||
|
||||
| Channel | Inferred physical side | Status | Object slots | Compact scan records |
|
||||
| --- | --- | --- | --- | --- |
|
||||
| A | right front | `240`, 16 B | `241–24F`, 15 × 24 B | `270–277`, 8 × 32 B, seven records each |
|
||||
| B | left front | `278`, 8 B | `279–287`, 15 × 24 B | `288–28F`, 8 × 32 B, seven records each |
|
||||
|
||||
Both channels run at about 20 Hz and validate with the HKG CAN-FD checksum in
|
||||
the sampled Ioniq 5, Palisade 2023, and EV6 data. All 288,021 checked frames
|
||||
from five complete segment samples passed. A synchronized Palisade
|
||||
parking-garage sweep strongly assigns A to the right-front sensor and B to
|
||||
the left-front sensor: A alone retained close objects at large negative
|
||||
lateral position, while B alone retained the corresponding positive-side
|
||||
objects. This is strong route/video evidence, but a controlled sensor
|
||||
occlusion remains the final physical confirmation.
|
||||
|
||||
The 24-byte tracked-object record is:
|
||||
|
||||
| Signal | Bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `UNKNOWN_CATEGORY` | 24 | 2 | raw | Stable within continuous same-slot tracks and changes mainly when a spatially different target replaces the slot. All values 0–3 occur on active objects, so this is not lifecycle or validity. |
|
||||
| `RCS` | 56 | 7 signed | 1 / 0 | Strong shared-core identification; raw unit uncalibrated. |
|
||||
| `LONG_DIST` | 63 | 13 | 0.05 / 0 m | Confirmed vehicle-frame longitudinal position. |
|
||||
| `LAT_DIST` | 76 | 11 signed | 0.05 / 0 m | Confirmed vehicle-frame lateral position. |
|
||||
| `UNKNOWN_BIT_87` | 87 | 1 | raw | Active unknown. |
|
||||
| `REL_SPEED` | 88 | 10 signed | 0.2 / 0 m/s | Confirmed longitudinal relative velocity. Four held-out Palisade samples exercised bit 97 independently of bit 96 at 351–357 m; consecutive distance changes matched approximately -52 m/s rather than the false positive speed produced by the former 9-bit decode. |
|
||||
| `REL_LAT_SPEED` | 98 | 10 signed | 0.05 / 0 m/s | Confirmed lateral relative velocity. |
|
||||
| `UNKNOWN_BITS_108_117` | 108 | 10 | raw | Former centered acceleration candidate. Its correlations with consecutive `REL_SPEED` derivatives were -0.445, -0.043, -0.009, -0.111, and -0.124 across five Ioniq 5, Palisade, and EV6 samples, so the physical name and scale were falsified. |
|
||||
|
||||
Empty object slots use the exact `LONG_DIST=204.7 m` (`0xFFE`) sentinel;
|
||||
`UNKNOWN_CATEGORY=0` must not be used as an empty test. This correction was
|
||||
validated on five Ioniq 5, Palisade 2023, and EV6 samples. Including
|
||||
category-zero objects produced longitudinal position-derivative correlations
|
||||
of 0.949, 0.603, 0.962, 0.948, and 0.967 across those samples.
|
||||
|
||||
The position decode was independently checked against simultaneous
|
||||
`210–21F` front-radar targets. The 116 unambiguous A matches and 86 B matches
|
||||
had longitudinal/lateral correlations of 0.999/0.991 and 0.999/0.993,
|
||||
respectively, with about 0.3 m mean absolute error. Across 14,939 consecutive
|
||||
same-slot updates, decoded longitudinal and lateral velocity correlated with
|
||||
position derivatives at 0.964 and 0.944. No lifecycle field has been
|
||||
identified, and the former acceleration candidate failed derivative checks.
|
||||
The remaining header, classification, dimension, ID, and health fields stay
|
||||
bit-preserved rather than borrowing names from the related front-radar core.
|
||||
|
||||
The first payload byte after the counter in `0x278` is
|
||||
`CHANNEL_A_OBJECT_COUNT`. Across 5,999 synchronized cycles on five Ioniq 5,
|
||||
Palisade, and EV6 samples, the number of non-sentinel `0x241–0x24F` slots equaled
|
||||
`min(CHANNEL_A_OBJECT_COUNT, 15)` exactly. It ranged from 0–22; values above
|
||||
15 prove it is the pre-truncation internal candidate count rather than merely
|
||||
the number of exported slots. The `0x240` status payload remains unresolved;
|
||||
notably, it was entirely zero after checksum/counter on one Palisade and one
|
||||
EV6 sample while being active on the other three routes.
|
||||
|
||||
The compact arrays are not alternate encodings of that object count. Depending
|
||||
on route and channel, 11–55 of their 56 bins were non-sentinel in a cycle,
|
||||
versus at most 15 exported tracked objects; count correlations ranged from
|
||||
weak to moderate and were never exact.
|
||||
|
||||
The compact 32-bit scan record is:
|
||||
|
||||
| Record field | Relative bit | Size | Scale | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `DISTANCE_CANDIDATE` | 0 | 13 | 0.05 m | Range-like distribution and 400 m endpoint; no synchronized object association, so physical distance is not confirmed. |
|
||||
| `RESERVED` | 13 | 3 | raw | Preserve. |
|
||||
| `PROPERTY` | 16 | 7 | raw | Active categorical unknown; sampled values were quantized. |
|
||||
| `AUX` | 23 | 9 | raw | Active unknown; velocity and split flag/state hypotheses were falsified. |
|
||||
|
||||
Record 0 starts at message bit 24, message bit 56 is a frame-status byte, and
|
||||
records 1–6 start at bits 64, 96, 128, 160, 192, and 224. Unused records
|
||||
commonly contain `0x010D1F40`, with AUX variants such as `0x018D1F40`; its
|
||||
low-field component is the 400 m endpoint. Contemporaneous range matching was no
|
||||
better than the opposite channel or a 500 ms-shifted control. Each of the 56
|
||||
record positions instead has a characteristic occupancy/range profile, making
|
||||
this a strong fixed scan/bin-array candidate rather than 56 freely assigned
|
||||
tracked detections. A direct linear record-index-to-angle search on Ioniq 5
|
||||
and Palisade data also performed no better than a 500 ms-shifted control.
|
||||
Decoding the low field, physical ordering, `PROPERTY`, and `AUX` remains open.
|
||||
|
||||
## 3D0-3D4 front-radar auxiliary scan records
|
||||
|
||||
This optional list accompanies `3A5–3C4` on sampled EV9 and Ioniq 9 routes.
|
||||
Five 32-byte messages contain seven record positions each; the final three
|
||||
positions of `3D4` are outside the 32-position list. The repeated
|
||||
`0xC8782EE0` value and, on EV9, zero are unused records. All 12,090 checked
|
||||
message frames passed the HKG CAN-FD checksum and carried the same cycle
|
||||
counter as `3A5–3C4`.
|
||||
|
||||
| Record field | Relative bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `DISTANCE_CANDIDATE` | 0 | 12 | 0.05 / 0 m | Range-like distribution and endpoint, but no synchronized tracked-object match; not confirmed. |
|
||||
| `FLAGS_UNKNOWN` | 12 | 4 | raw | Active values 0–2 in the sampled routes; semantics unknown. |
|
||||
| `UNKNOWN_BYTE_16` | 16 | 8 | raw | Former radial-speed candidate was falsified. |
|
||||
| `UNKNOWN_BYTE_24` | 24 | 8 | raw | Former azimuth candidate was falsified. |
|
||||
|
||||
Packing positions match the corner scan records: record 0 at message bit 24,
|
||||
one alignment byte at bit 56, then records 1–6 at bits 64 through 224. However,
|
||||
the exact corner `13/3/7/9` field split is not transferred onto this
|
||||
`12/4/8/8` layout without evidence.
|
||||
|
||||
The stronger synchronized test falsified three tempting interpretations:
|
||||
|
||||
- Range-only, candidate polar-geometry, and direct record-index-to-track-index
|
||||
associations were no better than opposite or 500 ms-shifted controls.
|
||||
- `UNKNOWN_BYTE_16` had essentially zero correlation with `3A5` longitudinal
|
||||
or radial relative speed.
|
||||
- `UNKNOWN_BYTE_24` had essentially zero correlation with `3A5` target angle.
|
||||
|
||||
The 32 positions do show stable position-dependent occupancy and low-field
|
||||
profiles, so a fixed scan/bin or intermediate radar-array role remains
|
||||
plausible. They are not independent tracked objects, remain excluded from
|
||||
production RadarData, and are shown only as raw records in the debugger's
|
||||
`SIGNALS` table, never projected as geometry.
|
||||
|
||||
## 360-366 forward-camera lane/path family
|
||||
|
||||
These seven 32-byte messages belong to the forward camera. `362` already
|
||||
contains left/right lane-line confidence. `360`, `361`, `363`, and `364`
|
||||
carry changing lane/path polynomial-like geometry; their field boundaries are
|
||||
not yet decoded. `365` and `366` are commonly constant/default-filled,
|
||||
consistent with unavailable extra lane slots. Their presence or emptiness
|
||||
must not be interpreted as a radar hardware combination.
|
||||
|
||||
## Reserved/dead bits
|
||||
|
||||
Bits `26`, `29`, `39`, `55`, `117`, `136-137`, `143`, `171`, and
|
||||
`188-191` remain zero. No new signal location was found in the follow-up
|
||||
routes.
|
||||
|
||||
## Parser guidance
|
||||
|
||||
- Detect support per radar source instead of treating extension defaults as
|
||||
real measurements.
|
||||
- Treat zero dimensions as unavailable. Treat +180 as unavailable on non-rich
|
||||
platforms, but do not discard near-180 rich tracks without checking their
|
||||
other geometry and lifecycle fields.
|
||||
- Do not use any `NEW_SIGNAL_*` field for control or filtering until its
|
||||
meaning is independently validated.
|
||||
- `TRACK_QUALITY` and `RCS` are decoded for research and diagnostics, but
|
||||
neither has a calibrated physical/percentage scale suitable for control.
|
||||
- `REL_LAT_SPEED` is suitable for debugging cross traffic, but a production
|
||||
cross-traffic filter still needs lane-change validation and track history.
|
||||
Executable
+343
@@ -0,0 +1,343 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import concurrent.futures
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from collections import Counter, defaultdict
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from msgq.ipc_pyx import IpcError
|
||||
from opendbc.car.tests.routes import CarTestRoute, routes
|
||||
from openpilot.tools.radar.custom_routes import CUSTOM_ROUTES
|
||||
from openpilot.tools.radar.radar_helpers import (
|
||||
build_seen_address_map,
|
||||
get_radar_spec,
|
||||
is_exclusive_full_range_match,
|
||||
)
|
||||
|
||||
|
||||
REPLAY_PATH = os.path.join(os.path.dirname(__file__), "..", "replay", "replay")
|
||||
REPLAY_PLAYBACK_SPEED = "3.0"
|
||||
REPLAY_ALLOW_SERVICES = "can"
|
||||
DETECTION_TIMEOUT_SECONDS = 20.0
|
||||
DETECTION_MIN_HITS = 10
|
||||
SOCKET_WAIT_TIMEOUT_SECONDS = 10.0
|
||||
CAR_MODEL_WIDTH = 38
|
||||
RADAR_TYPE_WIDTH = 10
|
||||
|
||||
ANSI_RESET = "\033[0m"
|
||||
ANSI_BOLD = "\033[1m"
|
||||
ANSI_DIM = "\033[2m"
|
||||
ANSI_RED = "\033[31m"
|
||||
ANSI_GREEN = "\033[32m"
|
||||
ANSI_YELLOW = "\033[33m"
|
||||
ANSI_BLUE = "\033[34m"
|
||||
ANSI_MAGENTA = "\033[35m"
|
||||
ANSI_CYAN = "\033[36m"
|
||||
|
||||
RADAR_FAMILY_COLORS = {
|
||||
"RADAR_500_51F": ANSI_CYAN,
|
||||
"RADAR_210_21F": ANSI_YELLOW,
|
||||
"RADAR_3A5_3C4": ANSI_GREEN,
|
||||
"RADAR_602_611": ANSI_BLUE,
|
||||
}
|
||||
|
||||
|
||||
def build_arg_parser() -> argparse.ArgumentParser:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Replay Hyundai/Kia/Genesis test routes and auto-detect radar type.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
||||
)
|
||||
parser.add_argument("--routes", action="store_true",
|
||||
help="Use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
|
||||
parser.add_argument("--custom-routes", action="store_true",
|
||||
help="Use routes from tools/radar/custom_routes.py.")
|
||||
parser.add_argument("--route", action="append", default=[],
|
||||
help="Specific route(s) to test. If omitted, use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
|
||||
parser.add_argument("--data-dir", default=None,
|
||||
help="Optional local directory of route data to pass through to replay.")
|
||||
parser.add_argument("--timeout", type=float, default=DETECTION_TIMEOUT_SECONDS,
|
||||
help="Seconds to wait for a radar-family detection before giving up on a route.")
|
||||
parser.add_argument("--min-hits", type=int, default=DETECTION_MIN_HITS,
|
||||
help="Minimum CAN hits in a radar-family address range before considering it detected.")
|
||||
parser.add_argument("--playback", default=REPLAY_PLAYBACK_SPEED,
|
||||
help="Replay playback speed.")
|
||||
parser.add_argument("--limit", type=int, default=None,
|
||||
help="Only process the first N matching routes.")
|
||||
parser.add_argument("--jobs", type=int, default=1,
|
||||
help="Number of routes to process in parallel.")
|
||||
parser.add_argument("--prefix", default="auto-radar-detect",
|
||||
help="Base OPENPILOT_PREFIX to isolate replay sockets.")
|
||||
return parser
|
||||
|
||||
|
||||
def get_hkg_routes() -> list[CarTestRoute]:
|
||||
hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_")
|
||||
return [
|
||||
route for route in routes
|
||||
if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes)
|
||||
]
|
||||
|
||||
|
||||
def get_selected_routes(args: argparse.Namespace) -> list[CarTestRoute]:
|
||||
selected_sources = int(bool(args.route)) + int(bool(args.routes)) + int(bool(args.custom_routes))
|
||||
if selected_sources > 1:
|
||||
raise ValueError("Use only one of --route, --routes, or --custom-routes.")
|
||||
|
||||
if args.route:
|
||||
route_map = {route.route: route for route in get_hkg_routes()}
|
||||
selected = []
|
||||
for route_name in args.route:
|
||||
selected.append(route_map.get(route_name, CarTestRoute(route_name, None)))
|
||||
return selected[:args.limit] if args.limit is not None else selected
|
||||
|
||||
if args.custom_routes:
|
||||
selected = [CarTestRoute(route.route, route.car_model) for route in CUSTOM_ROUTES]
|
||||
return selected[:args.limit] if args.limit is not None else selected
|
||||
|
||||
selected = get_hkg_routes()
|
||||
return selected[:args.limit] if args.limit is not None else selected
|
||||
|
||||
|
||||
def terminate_process(proc: subprocess.Popen) -> None:
|
||||
if proc.poll() is not None:
|
||||
return
|
||||
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
proc.kill()
|
||||
proc.wait(timeout=5)
|
||||
|
||||
|
||||
def start_replay(route: str, prefix: str, args: argparse.Namespace) -> subprocess.Popen:
|
||||
cmd = [
|
||||
REPLAY_PATH,
|
||||
"--no-vipc",
|
||||
"--no-loop",
|
||||
"--playback", args.playback,
|
||||
"--allow", REPLAY_ALLOW_SERVICES,
|
||||
"--prefix", prefix,
|
||||
]
|
||||
if args.data_dir:
|
||||
cmd.extend(["--data_dir", args.data_dir])
|
||||
cmd.append(route)
|
||||
|
||||
env = os.environ.copy()
|
||||
env["OPENPILOT_PREFIX"] = prefix
|
||||
return subprocess.Popen(
|
||||
cmd,
|
||||
cwd=os.path.dirname(REPLAY_PATH),
|
||||
env=env,
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
preexec_fn=os.setsid if os.name != "nt" else None,
|
||||
)
|
||||
|
||||
|
||||
def stop_replay(proc: subprocess.Popen) -> None:
|
||||
if proc.poll() is not None:
|
||||
return
|
||||
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
else:
|
||||
proc.terminate()
|
||||
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
else:
|
||||
proc.kill()
|
||||
proc.wait(timeout=5)
|
||||
|
||||
|
||||
def wait_for_can_socket(prefix: str, timeout: float) -> messaging.SubSocket:
|
||||
socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can")
|
||||
started = time.monotonic()
|
||||
while True:
|
||||
if os.path.exists(socket_path):
|
||||
break
|
||||
if time.monotonic() - started > timeout:
|
||||
raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}")
|
||||
time.sleep(0.1)
|
||||
|
||||
while True:
|
||||
try:
|
||||
return messaging.sub_sock("can", conflate=False, timeout=100)
|
||||
except IpcError:
|
||||
if time.monotonic() - started > timeout:
|
||||
raise
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def detect_radar_family(route: CarTestRoute, route_idx: int, args: argparse.Namespace) -> dict:
|
||||
prefix = f"{args.prefix}-{os.getpid()}-{route_idx}"
|
||||
os.environ["OPENPILOT_PREFIX"] = prefix
|
||||
messaging.reset_context()
|
||||
proc = start_replay(route.route, prefix, args)
|
||||
logcan = wait_for_can_socket(prefix, min(args.timeout, SOCKET_WAIT_TIMEOUT_SECONDS))
|
||||
counts = Counter()
|
||||
buses = defaultdict(set)
|
||||
seen_addresses = build_seen_address_map()
|
||||
started = time.monotonic()
|
||||
detected = None
|
||||
|
||||
try:
|
||||
while True:
|
||||
if proc.poll() is not None and (time.monotonic() - started) > 1.0:
|
||||
break
|
||||
if time.monotonic() - started > args.timeout:
|
||||
break
|
||||
|
||||
msgs = messaging.drain_sock(logcan, wait_for_one=True)
|
||||
for msg in msgs:
|
||||
for can_msg in msg.can:
|
||||
radar_spec = get_radar_spec(can_msg.address)
|
||||
if radar_spec is not None:
|
||||
counts[radar_spec.name] += 1
|
||||
buses[radar_spec.name].add(can_msg.src)
|
||||
seen_addresses[radar_spec.name].add(can_msg.address)
|
||||
if counts[radar_spec.name] >= args.min_hits and is_exclusive_full_range_match(radar_spec, seen_addresses):
|
||||
detected = radar_spec.name
|
||||
if detected is not None:
|
||||
break
|
||||
if detected is not None:
|
||||
break
|
||||
if detected is not None:
|
||||
break
|
||||
finally:
|
||||
stop_replay(proc)
|
||||
|
||||
dominant_family = detected
|
||||
|
||||
return {
|
||||
"route": route.route,
|
||||
"car_model": str(route.car_model) if route.car_model is not None else "UNKNOWN",
|
||||
"detected": dominant_family or "NONE",
|
||||
"counts": dict(counts),
|
||||
"buses": {name: sorted(bus_set) for name, bus_set in buses.items()},
|
||||
"seen_addresses": {name: sorted(addresses) for name, addresses in seen_addresses.items()},
|
||||
}
|
||||
|
||||
|
||||
def print_results(results: list[dict]) -> None:
|
||||
print(f"{'Car Model':<{CAR_MODEL_WIDTH}} {'Detected':<{RADAR_TYPE_WIDTH}} Route")
|
||||
print("-" * (CAR_MODEL_WIDTH + RADAR_TYPE_WIDTH + 9 + 60))
|
||||
for result in results:
|
||||
detected_plain = "" if result["detected"] == "NONE" else result["detected"]
|
||||
model = format_car_model(result["car_model"])[:CAR_MODEL_WIDTH]
|
||||
detected = f"{detected_plain:<{RADAR_TYPE_WIDTH}}"
|
||||
if supports_color() and detected_plain:
|
||||
detected = f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain))
|
||||
print(f"{model:<{CAR_MODEL_WIDTH}} {detected} {result['route']}")
|
||||
|
||||
|
||||
def supports_color() -> bool:
|
||||
return sys.stdout.isatty() and os.getenv("TERM") not in (None, "dumb")
|
||||
|
||||
|
||||
def format_car_model(car_model: object) -> str:
|
||||
return str(car_model) if car_model is not None else "UNKNOWN"
|
||||
|
||||
|
||||
def colorize_detected(detected: str) -> str:
|
||||
if detected == "NONE":
|
||||
return ""
|
||||
|
||||
if not supports_color():
|
||||
return detected
|
||||
|
||||
color = RADAR_FAMILY_COLORS.get(detected, ANSI_MAGENTA)
|
||||
return f"{color}{detected}{ANSI_RESET}"
|
||||
|
||||
|
||||
def print_progress_line(index: int, total: int, route: CarTestRoute, detected: str) -> None:
|
||||
left = f"[{index}/{total}]"
|
||||
model = format_car_model(route.car_model)[:CAR_MODEL_WIDTH]
|
||||
detected_plain = "" if detected == "NONE" else detected
|
||||
route_text = route.route
|
||||
if supports_color():
|
||||
left = f"{ANSI_DIM}{left}{ANSI_RESET}"
|
||||
model = f"{ANSI_CYAN}{model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}"
|
||||
detected_text = colorize_detected(detected_plain).ljust(len(colorize_detected(detected_plain)))
|
||||
route_text = f"{ANSI_DIM}{route_text}{ANSI_RESET}"
|
||||
else:
|
||||
model = f"{model:<{CAR_MODEL_WIDTH}}"
|
||||
detected_text = detected_plain
|
||||
|
||||
detected_text = f"{detected_plain:<{RADAR_TYPE_WIDTH}}" if not supports_color() else (
|
||||
f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain))
|
||||
)
|
||||
|
||||
print(f"{left} {model} {detected_text} {route_text}")
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = build_arg_parser().parse_args(sys.argv[1:])
|
||||
try:
|
||||
selected_routes = get_selected_routes(args)
|
||||
except ValueError as e:
|
||||
print(str(e), file=sys.stderr)
|
||||
return 2
|
||||
if not selected_routes:
|
||||
print("No matching routes found.", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
results = []
|
||||
total = len(selected_routes)
|
||||
jobs = max(1, min(args.jobs, total))
|
||||
|
||||
header_model = "Car Model"
|
||||
header_detected = "Radar"
|
||||
if supports_color():
|
||||
header_model = f"{ANSI_BOLD}{header_model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}"
|
||||
header_detected = f"{ANSI_BOLD}{header_detected:<{RADAR_TYPE_WIDTH}}{ANSI_RESET}"
|
||||
else:
|
||||
header_model = f"{header_model:<{CAR_MODEL_WIDTH}}"
|
||||
header_detected = f"{header_detected:<{RADAR_TYPE_WIDTH}}"
|
||||
print(f" {header_model} {header_detected} Route")
|
||||
|
||||
if jobs == 1:
|
||||
for idx, route in enumerate(selected_routes):
|
||||
result = detect_radar_family(route, idx, args)
|
||||
print_progress_line(idx + 1, total, route, result['detected'])
|
||||
results.append((idx, result))
|
||||
else:
|
||||
with concurrent.futures.ProcessPoolExecutor(max_workers=jobs) as executor:
|
||||
future_to_job = {
|
||||
executor.submit(detect_radar_family, route, idx, args): (idx, route)
|
||||
for idx, route in enumerate(selected_routes)
|
||||
}
|
||||
completed = 0
|
||||
for future in concurrent.futures.as_completed(future_to_job):
|
||||
idx, route = future_to_job[future]
|
||||
result = future.result()
|
||||
completed += 1
|
||||
print_progress_line(completed, total, route, result['detected'])
|
||||
results.append((idx, result))
|
||||
|
||||
results = sorted(
|
||||
(result for _, result in results),
|
||||
key=lambda result: (result["car_model"], result["route"]),
|
||||
)
|
||||
|
||||
print()
|
||||
print_results(results)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Executable
+68
@@ -0,0 +1,68 @@
|
||||
#!/usr/bin/env python3
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CustomRoute:
|
||||
car_model: str
|
||||
route: str
|
||||
|
||||
|
||||
CUSTOM_ROUTES = [
|
||||
CustomRoute("HYUNDAI_ELANTRA_HEV_2024", "07a48901db7b2503|0000012d--61aba9f832"),
|
||||
CustomRoute("HYUNDAI_IONIQ_5", "c5a5f79df9b6a084/0000005d--24c0a7a7ee"),
|
||||
CustomRoute("HYUNDAI_IONIQ_5", "90950642f47cf05b/00000681--6a904aaa98"),
|
||||
CustomRoute("HYUNDAI_IONIQ_5", "102161de13e822d1/0000000d--f74a52a624"),
|
||||
CustomRoute("HYUNDAI_PALISADE_2023", "17ef028fec0ccf81/00000010--dba8455015"),
|
||||
CustomRoute("HYUNDAI_PALISADE_2023", "6b8052042ba3ee05/00000199--3ed3fb5799"),
|
||||
CustomRoute("KIA_EV6", "455a0ab75ce5c1e0/000000d1--098f32028c"),
|
||||
CustomRoute("KIA_EV6", "6d2092783bf67457/0000008c--5caf525813"),
|
||||
CustomRoute("KIA_EV6", "4961cb0f7bdd77a2/00000105--b636ed2f65"),
|
||||
CustomRoute("KIA_K8_HEV_1ST_GEN", "78ad5150de133637|2023-09-13--16-15-57"),
|
||||
CustomRoute("GENESIS_GV60_EV_1ST_GEN", "b1e441e63f1c99dd|0000009d--14cde5aeaf"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000187--ce36ae7c11"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000191--e5c3ffb55b"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "ce8a3764a8bb9141/000000f6--775693a6d6"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "ce8a3764a8bb9141/00000100--ba8bd692f5"),
|
||||
CustomRoute("HYUNDAI_KONA_2ND_GEN", "32025f26789d8fab/00000022--a499e8ffa3"),
|
||||
CustomRoute("HYUNDAI_KONA_EV_2ND_GEN", "1618132d68afc876/00000021--bf0f957649"),
|
||||
CustomRoute("HYUNDAI_KONA_HEV_2ND_GEN", "97ca61196eb73e0d/00000052--4555329470"),
|
||||
CustomRoute("HYUNDAI_SANTA_FE_HEV_5TH_GEN", "d54302f1d5e7a7cc|00000656--5dae9f54a7"),
|
||||
CustomRoute("HYUNDAI_SONATA_2024", "4267ea8a353cdb36/00000262--8a427003c7"),
|
||||
CustomRoute("KIA_CARNIVAL_HEV_4TH_GEN_2026", "7b8cc7bb46000e53/0000000b--2abacaff78"),
|
||||
CustomRoute("KIA_EV6_2025", "48c27f77f9fd1a9b|00000199--193ee1ba20"),
|
||||
CustomRoute("KIA_EV9", "ccfd4a1af758ee73/00000091--7fa49719a5"),
|
||||
CustomRoute("KIA_K4_2025", "baf39eeaba1217ca/00000002--b36e3fa031"),
|
||||
CustomRoute("KIA_K4_2025", "ac30936e794b297a/00000021--dde25fcfda/24"),
|
||||
CustomRoute("KIA_K5_2025", "c4a804b067623789/0000007c--163f831540"),
|
||||
CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032d--8379197bd3"),
|
||||
CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032c--0055eeee96"),
|
||||
CustomRoute("KIA_NIRO_EV_2ND_GEN", "cdde60a79a049bf4/00000002--f448ca40b4"),
|
||||
CustomRoute("KIA_NIRO_HEV_2ND_GEN", "0d4257d1c6741384/00000067--42230e4b8d"),
|
||||
CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000003--d0591c14ec"),
|
||||
CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000001--18955a967e"),
|
||||
CustomRoute("KIA_CEED_PHEV", "26064e18b24ae44c/00000000--4eb95fc137"),
|
||||
CustomRoute("HYUNDAI_IONIQ", "ac27c9808ac91710/00000002--e0aab81e4d"),
|
||||
CustomRoute("KIA_K7_2017", "74fbff45aa20fe9e/00000010--6f173d5799"),
|
||||
CustomRoute("KIA_NIRO_EV", "b27d9eafeb61976e/00000233--03a9c0858c"),
|
||||
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000157--ecaf77a801"),
|
||||
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000158--f7e70e1435"),
|
||||
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000154--3471fc4ab3"),
|
||||
CustomRoute("KIA_SORENTO_4TH_GEN", "bf42073ef09e0af2/00000017--3edc1f8259"),
|
||||
CustomRoute("KIA_SORENTO_HEV_4TH_GEN", "833262b0c9e4016a/0000004a--0227a058e3"),
|
||||
CustomRoute("HYUNDAI_PALISADE", "662bedbf8453b81e/00000124--7dc358f20c"),
|
||||
CustomRoute("HYUNDAI_SONATA", "8f52823c702cb300/00000177--177d1aa6ff"),
|
||||
CustomRoute("HYUNDAI_SONATA_HYBRID", "e3ae9e987a2a4e57/0000002b--30a6963e88"),
|
||||
CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000031--4cb0d8aca5"),
|
||||
CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000042--302b371266"),
|
||||
CustomRoute("HYUNDAI_IONIQ_HEV_2022", "4a975fc1d9e71801/00000005--50d176008a"),
|
||||
CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/00000014--25fc6b216f"),
|
||||
CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/0000000e--d3b19ad6f6/10"),
|
||||
CustomRoute("HYUNDAI_SANTA_CRUZ_2025", "6e7904b03a4aafc2/00000010--31034184b6"),
|
||||
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "26da1db30eff4fcc/00000061--a60470e363"),
|
||||
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "a6d25e95d936fdc4/000002ed--89d7944c52"),
|
||||
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "6722665fbbf2a644/00000534--4ab7b733c5"),
|
||||
CustomRoute("HYUNDAI_TUCSON_HEV_2025", "5868ec006e2bb61e/00000021--7d10df95e8"),
|
||||
CustomRoute("KIA_SPORTAGE_5TH_GEN", "ce05e32158479f42/000003f4--9543087719"),
|
||||
CustomRoute("KIA_SPORTAGE_HEV_2026", "343d5e350abaedcf/00000020--1c864750b6"),
|
||||
]
|
||||
+329
@@ -0,0 +1,329 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from collections import Counter, defaultdict
|
||||
from collections.abc import Iterable, Iterator
|
||||
from pathlib import Path
|
||||
import re
|
||||
import sys
|
||||
from urllib.parse import unquote
|
||||
|
||||
|
||||
SUNNYPILOT_ROOT = Path(__file__).resolve().parents[4]
|
||||
if (SUNNYPILOT_ROOT / "tools/lib/logreader.py").exists():
|
||||
sys.path.insert(0, str(SUNNYPILOT_ROOT))
|
||||
|
||||
|
||||
DEFAULT_ROUTES = """
|
||||
faa815c21279fef7/00000187--ce36ae7c11
|
||||
faa815c21279fef7/00000191--e5c3ffb55b
|
||||
ce8a3764a8bb9141/000000f6--775693a6d6
|
||||
ce8a3764a8bb9141/00000100--ba8bd692f5
|
||||
32025f26789d8fab/00000022--a499e8ffa3
|
||||
1618132d68afc876/00000021--bf0f957649
|
||||
97ca61196eb73e0d/00000052--4555329470
|
||||
d54302f1d5e7a7cc/00000656--5dae9f54a7
|
||||
4267ea8a353cdb36/00000262--8a427003c7
|
||||
7b8cc7bb46000e53/0000000b--2abacaff78
|
||||
48c27f77f9fd1a9b/00000199--193ee1ba20
|
||||
ccfd4a1af758ee73/00000091--7fa49719a5
|
||||
baf39eeaba1217ca/00000002--b36e3fa031
|
||||
ac30936e794b297a/00000021--dde25fcfda
|
||||
c4a804b067623789/0000007c--163f831540
|
||||
80b8e9a6ad0acec3/0000032d--8379197bd3
|
||||
80b8e9a6ad0acec3/0000032c--0055eeee96
|
||||
cdde60a79a049bf4/00000002--f448ca40b4
|
||||
0d4257d1c6741384/00000067--42230e4b8d
|
||||
343d5e350abaedcf/00000020--1c864750b6
|
||||
71e4e67d29034771/00000014--25fc6b216f
|
||||
71e4e67d29034771/0000000e--d3b19ad6f6
|
||||
07a48901db7b2503/0000012d--61aba9f832
|
||||
c5a5f79df9b6a084/0000005d--24c0a7a7ee
|
||||
90950642f47cf05b/00000681--6a904aaa98
|
||||
102161de13e822d1/0000000d--f74a52a624
|
||||
17ef028fec0ccf81/00000010--dba8455015
|
||||
6b8052042ba3ee05/00000199--3ed3fb5799
|
||||
464ac801173f6dca/00000382--27ce9e4ade
|
||||
455a0ab75ce5c1e0/000000d1--098f32028c
|
||||
6d2092783bf67457/0000008c--5caf525813
|
||||
4961cb0f7bdd77a2/00000105--b636ed2f65
|
||||
f9522b6cd6e4e621/00000170--ac6979bdda
|
||||
ed2717e2c1912f87/00000009--d550f4d5b6
|
||||
78ad5150de133637/2023-09-13--16-15-57
|
||||
6e7904b03a4aafc2/00000010--31034184b6
|
||||
26da1db30eff4fcc/00000061--a60470e363
|
||||
a6d25e95d936fdc4/000002ed--89d7944c52
|
||||
6722665fbbf2a644/00000534--4ab7b733c5
|
||||
477e0b6c352e8bba/0000000e--5e3a5c981c
|
||||
5868ec006e2bb61e/00000021--7d10df95e8
|
||||
ce05e32158479f42/000003f4--9543087719
|
||||
ac27c9808ac91710/00000002--e0aab81e4d
|
||||
74fbff45aa20fe9e/00000010--6f173d5799
|
||||
b27d9eafeb61976e/00000233--03a9c0858c
|
||||
f6324c8fb8b6f68d/00000058--5330471811
|
||||
e174e1a0b92263ed/00000001--18955a967e
|
||||
26064e18b24ae44c/00000000--4eb95fc137
|
||||
4709fe68b0cc9c0a/00000125--ed2a02e7a8
|
||||
4709fe68b0cc9c0a/0000012b--f29c054b05
|
||||
ce7eada09318e376/0000000e--0cc3d1c6cc
|
||||
662bedbf8453b81e/00000124--7dc358f20c
|
||||
4a975fc1d9e71801/00000005--50d176008a
|
||||
3e4f79d5bf692518/00000001--f53d01f8e3
|
||||
3e4f79d5bf692518/00000002--b18171f9a9
|
||||
810a7ca7cb31ed86/00000001--b6f9cdd134
|
||||
224d1e774b6acc2e/0000001d--0ae7d5fa70
|
||||
8f52823c702cb300/00000177--177d1aa6ff
|
||||
e3ae9e987a2a4e57/0000002b--30a6963e88
|
||||
7bc0de9da607c543/00000031--4cb0d8aca5
|
||||
7bc0de9da607c543/00000042--302b371266
|
||||
bf42073ef09e0af2/00000017--3edc1f8259
|
||||
833262b0c9e4016a/0000004a--0227a058e3
|
||||
3cce51ba4cc49950/0000003c--51ebc96f4c
|
||||
3cce51ba4cc49950/0000003e--d78c337cc5
|
||||
ad9840558640c31d/0000001a--d6cd4871c2
|
||||
a468064f19bc0267/00000001--6a0c7d2514
|
||||
a468064f19bc0267/00000012--1feb5188ec
|
||||
a468064f19bc0267/00000015--27b5b89e1b
|
||||
a468064f19bc0267/00000016--31baf70a9f
|
||||
a468064f19bc0267/00000017--c63c64192b
|
||||
518f8ad60a03f394/00000016--e0300496c8
|
||||
"""
|
||||
|
||||
|
||||
def import_logreader():
|
||||
for mod_name in ("openpilot.tools.lib.logreader", "tools.lib.logreader"):
|
||||
try:
|
||||
mod = __import__(mod_name, fromlist=["LogReader"])
|
||||
return mod.LogReader, mod.ReadMode
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
raise SystemExit(
|
||||
"Could not import LogReader. Run this from an openpilot/comma tooling environment, "
|
||||
+ "or install the package that provides openpilot.tools.lib.logreader."
|
||||
)
|
||||
|
||||
|
||||
def normalize_route(route: str) -> str:
|
||||
return unquote(route.strip()).replace("|", "/")
|
||||
|
||||
|
||||
def add_segment(route: str, segment: int | None) -> str:
|
||||
if segment is None:
|
||||
return route
|
||||
|
||||
parts = route.split("/")
|
||||
if len(parts) == 2:
|
||||
return f"{route}/{segment}"
|
||||
return route
|
||||
|
||||
|
||||
def parse_routes(text: str, segment: int | None) -> list[str]:
|
||||
oneboxes = re.findall(r"onebox=([^) \t\n\r]+)", text)
|
||||
routes = oneboxes if oneboxes else [line for line in text.splitlines() if line.strip() and not line.lstrip().startswith("#")]
|
||||
|
||||
deduped = dict.fromkeys(add_segment(normalize_route(route), segment) for route in routes)
|
||||
return list(deduped)
|
||||
|
||||
|
||||
def event_time_s(event, start_time_ns: int) -> float:
|
||||
return (event.logMonoTime - start_time_ns) / 1e9
|
||||
|
||||
|
||||
def can_messages(event) -> Iterable:
|
||||
which = event.which()
|
||||
if which == "can":
|
||||
return event.can
|
||||
if which == "sendcan":
|
||||
return event.sendcan
|
||||
return ()
|
||||
|
||||
|
||||
def similar_count(count: int, reference: float, tolerance: float) -> bool:
|
||||
if reference == 0:
|
||||
return count == 0
|
||||
return abs(count - reference) / reference <= tolerance
|
||||
|
||||
|
||||
def consecutive_ranges(address_counts: Counter[int], min_len: int, count_tolerance: float) -> Iterator[tuple[int, int]]:
|
||||
sorted_addresses = sorted(address_counts)
|
||||
if not sorted_addresses:
|
||||
return
|
||||
|
||||
start = prev = sorted_addresses[0]
|
||||
run_counts = [address_counts[start]]
|
||||
for addr in sorted_addresses[1:]:
|
||||
reference_count = sum(run_counts) / len(run_counts)
|
||||
if addr == prev + 1 and similar_count(address_counts[addr], reference_count, count_tolerance):
|
||||
run_counts.append(address_counts[addr])
|
||||
prev = addr
|
||||
continue
|
||||
|
||||
if prev - start + 1 >= min_len:
|
||||
yield start, prev
|
||||
start = prev = addr
|
||||
run_counts = [address_counts[addr]]
|
||||
|
||||
if prev - start + 1 >= min_len:
|
||||
yield start, prev
|
||||
|
||||
|
||||
def parse_buses(buses: str) -> set[int]:
|
||||
return {int(bus.strip(), 0) for bus in buses.split(",") if bus.strip()}
|
||||
|
||||
|
||||
def scan_route(LogReader, ReadMode, route: str, skip_s: float, duration_s: float | None,
|
||||
include_sendcan: bool, buses: set[int]) -> tuple[dict[int, Counter[int]], float]:
|
||||
counts_by_bus: dict[int, Counter[int]] = defaultdict(Counter)
|
||||
start_time_ns = None
|
||||
end_s = None if duration_s is None else skip_s + duration_s
|
||||
last_scanned_t = skip_s
|
||||
|
||||
for event in LogReader(route, default_mode=ReadMode.RLOG):
|
||||
if start_time_ns is None:
|
||||
start_time_ns = event.logMonoTime
|
||||
|
||||
t = event_time_s(event, start_time_ns)
|
||||
if t < skip_s:
|
||||
continue
|
||||
if end_s is not None and t > end_s:
|
||||
break
|
||||
last_scanned_t = t
|
||||
|
||||
if event.which() == "sendcan" and not include_sendcan:
|
||||
continue
|
||||
|
||||
for msg in can_messages(event):
|
||||
if msg.src not in buses:
|
||||
continue
|
||||
counts_by_bus[msg.src][msg.address] += 1
|
||||
|
||||
return counts_by_bus, max(last_scanned_t - skip_s, 0.0)
|
||||
|
||||
|
||||
def range_freq(counts: Counter[int], start: int, end: int, duration_s: float) -> float:
|
||||
if duration_s <= 0:
|
||||
return 0.0
|
||||
|
||||
freqs = [counts[addr] / duration_s for addr in range(start, end + 1)]
|
||||
return sum(freqs) / len(freqs)
|
||||
|
||||
|
||||
def fmt_addr(addr: int) -> str:
|
||||
return f"0x{addr:X}"
|
||||
|
||||
|
||||
def sorted_range_counts(counts: Counter[tuple[int | None, int, int]]) -> list[tuple[tuple[int | None, int, int], int]]:
|
||||
return sorted(counts.items(), key=lambda item: (-((item[0][2] - item[0][1]) + 1), -item[1], -1 if item[0][0] is None else item[0][0], item[0][1]))
|
||||
|
||||
|
||||
def print_range(bus: int | None, start: int, end: int, count: int, avg_hz: float, example: str | None = None, bus_label: str | None = None) -> None:
|
||||
bus_label = bus_label or ("all buses" if bus is None else f"bus {bus}")
|
||||
example_text = "" if example is None else f" e.g. {example}"
|
||||
print(f"{count:>4}x {bus_label:>8} {fmt_addr(start)}-{fmt_addr(end)} len={end - start + 1:<3} freq={avg_hz:>6.1f} Hz{example_text}", flush=True)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Find consecutive CAN address ranges observed after a route warm-up period.")
|
||||
parser.add_argument("input", nargs="?", help="Optional route, onebox URL, or file containing onebox URLs/routes. Defaults to the pasted side-quest routes.")
|
||||
parser.add_argument("--segment", type=int, default=1, help="Segment to append to route identifiers that do not already include one. Defaults to 1.")
|
||||
parser.add_argument("--all-segments", action="store_true", help="Do not append a segment; scan all segments selected by each identifier.")
|
||||
parser.add_argument("--skip-s", type=float, default=60.0, help="Seconds to skip from the start of each loaded route/segment.")
|
||||
parser.add_argument("--duration-s", type=float, default=None, help="Optional scan duration after --skip-s. Defaults to the rest of the log.")
|
||||
parser.add_argument("--min-len", type=int, default=4, help="Minimum consecutive address range length.")
|
||||
parser.add_argument("--count-tolerance", type=float, default=0.2, help="Maximum relative message-count difference within a range. Defaults to 0.2.")
|
||||
parser.add_argument("--buses", default="0,1,2", help="Comma-separated CAN buses to scan. Defaults to 0,1,2.")
|
||||
parser.add_argument("--include-sendcan", action="store_true", help="Include sendcan events in addition to received can events.")
|
||||
parser.add_argument("--merge-buses", "--no-bus", action="store_true", help="Merge addresses from all CAN buses before finding ranges.")
|
||||
parser.add_argument("--split-summary-buses", action="store_true", help="Keep aggregate summary counts separate per bus.")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.input:
|
||||
input_path = Path(args.input)
|
||||
text = input_path.read_text() if input_path.exists() else args.input
|
||||
else:
|
||||
text = DEFAULT_ROUTES
|
||||
routes = parse_routes(text, None if args.all_segments else args.segment)
|
||||
LogReader, ReadMode = import_logreader()
|
||||
buses = parse_buses(args.buses)
|
||||
|
||||
counts: Counter[tuple[int | None, int, int]] = Counter()
|
||||
freq_sums: Counter[tuple[int | None, int, int]] = Counter()
|
||||
examples: dict[tuple[int | None, int, int], str] = {}
|
||||
failed: list[tuple[str, str]] = []
|
||||
|
||||
for idx, route in enumerate(routes, 1):
|
||||
print(f"[{idx:>3}/{len(routes)}] scanning {route}", file=sys.stderr, flush=True)
|
||||
try:
|
||||
counts_by_bus, scanned_duration_s = scan_route(LogReader, ReadMode, route, args.skip_s, args.duration_s, args.include_sendcan, buses)
|
||||
except Exception as e:
|
||||
failed.append((route, f"{type(e).__name__}: {e}"))
|
||||
continue
|
||||
|
||||
if args.merge_buses:
|
||||
merged = Counter()
|
||||
for address_counts in counts_by_bus.values():
|
||||
merged.update(address_counts)
|
||||
range_sources = {None: merged}
|
||||
else:
|
||||
range_sources = counts_by_bus
|
||||
|
||||
route_ranges = set()
|
||||
route_freqs = {}
|
||||
for bus, address_counts in range_sources.items():
|
||||
for start, end in consecutive_ranges(address_counts, args.min_len, args.count_tolerance):
|
||||
route_ranges.add((bus, start, end))
|
||||
route_freqs[(bus, start, end)] = range_freq(address_counts, start, end, scanned_duration_s)
|
||||
|
||||
per_route_counts: Counter[tuple[int | None, int, int]] = Counter(route_ranges)
|
||||
print(f"\n{route}", flush=True)
|
||||
for (bus, start, end), route_count in sorted_range_counts(per_route_counts):
|
||||
print_range(bus, start, end, route_count, route_freqs[(bus, start, end)])
|
||||
|
||||
for range_key in route_ranges:
|
||||
counts[range_key] += 1
|
||||
freq_sums[range_key] += route_freqs[range_key]
|
||||
examples.setdefault(range_key, route)
|
||||
|
||||
print(f"\nScanned routes: {len(routes) - len(failed)}/{len(routes)}", flush=True)
|
||||
print(f"Minimum range length: {args.min_len}", flush=True)
|
||||
print(f"Count tolerance: {args.count_tolerance:g}", flush=True)
|
||||
print(f"Skipped first: {args.skip_s:g}s", flush=True)
|
||||
print(f"Buses: {','.join(str(bus) for bus in sorted(buses))}", flush=True)
|
||||
if args.duration_s is not None:
|
||||
print(f"Scan duration: {args.duration_s:g}s", flush=True)
|
||||
print(flush=True)
|
||||
|
||||
if args.split_summary_buses:
|
||||
for (bus, start, end), count in sorted_range_counts(counts):
|
||||
avg_hz = freq_sums[(bus, start, end)] / count
|
||||
print_range(bus, start, end, count, avg_hz, examples[(bus, start, end)])
|
||||
else:
|
||||
combined_counts: Counter[tuple[int, int]] = Counter()
|
||||
combined_freq_sums: Counter[tuple[int, int]] = Counter()
|
||||
combined_examples: dict[tuple[int, int], str] = {}
|
||||
combined_buses: dict[tuple[int, int], set[int]] = defaultdict(set)
|
||||
|
||||
for (bus, start, end), count in counts.items():
|
||||
range_key = (start, end)
|
||||
combined_counts[range_key] += count
|
||||
combined_freq_sums[range_key] += freq_sums[(bus, start, end)]
|
||||
combined_examples.setdefault(range_key, examples[(bus, start, end)])
|
||||
if bus is not None:
|
||||
combined_buses[range_key].add(bus)
|
||||
|
||||
sorted_combined = sorted(combined_counts.items(), key=lambda item: (-((item[0][1] - item[0][0]) + 1), -item[1], item[0][0]))
|
||||
for (start, end), count in sorted_combined:
|
||||
avg_hz = combined_freq_sums[(start, end)] / count
|
||||
buses_label = "buses " + ",".join(str(bus) for bus in sorted(combined_buses[(start, end)]))
|
||||
print_range(None, start, end, count, avg_hz, combined_examples[(start, end)], bus_label=buses_label)
|
||||
|
||||
if failed:
|
||||
print("\nFailed routes:", file=sys.stderr)
|
||||
for route, error in failed:
|
||||
print(f" {route}: {error}", file=sys.stderr)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+232
@@ -0,0 +1,232 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import os
|
||||
import tempfile
|
||||
from dataclasses import dataclass
|
||||
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.dbc.generator.hyundai.hyundai_radar_3a5_3c4 import RADAR_3A5_3C4_MESSAGE_COMMENT, RADAR_3A5_3C4_SIGNAL_COMMENTS
|
||||
|
||||
|
||||
RADAR_500_51F_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
|
||||
SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
|
||||
SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
|
||||
SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
|
||||
SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
|
||||
SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
|
||||
SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
|
||||
"""
|
||||
|
||||
RADAR_3A5_3C4_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 24 RADAR
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ STATE_ALT : 25|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ MOTION_STATE : 28|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ TRACK_COUNTER : 31|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ TRACK_QUALITY : 38|7@0+ (1,0) [0|127] "" XXX
|
||||
SG_ AGE : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COAST_AGE : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ STATE : 54|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ RCS : 62|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ LONG_DIST : 63|13@1+ (0.05,0) [0|409.55] "m" XXX
|
||||
SG_ LAT_DIST : 76|12@1- (0.05,0) [-102.4|102.35] "m" XXX
|
||||
SG_ REL_SPEED : 88|14@1- (0.01,0) [-81.92|81.91] "m/s" XXX
|
||||
SG_ NEW_SIGNAL_4 : 103|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ REL_LAT_SPEED : 104|13@1- (0.01,0) [-40.96|40.95] "m/s" XXX
|
||||
SG_ REL_ACCEL : 118|10@1- (0.02,0) [-10.24|10.22] "m/s^2" XXX
|
||||
SG_ NEW_SIGNAL_18 : 129|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ NEW_SIGNAL_5 : 135|6@0+ (1,0) [0|32] "" XXX
|
||||
SG_ WIDTH : 138|5@1+ (0.1,0) [0|3.1] "m" XXX
|
||||
SG_ LENGTH : 151|8@0+ (0.1,0) [0|25.5] "m" XXX
|
||||
SG_ ABS_SPEED : 152|10@1+ (0.1,0) [0|102.3] "m/s" XXX
|
||||
SG_ ORIENTATION_ANGLE : 162|9@1- (1,0) [-180|180] "deg" XXX
|
||||
SG_ NEW_SIGNAL_13 : 175|4@0+ (1,0) [0|10] "" XXX
|
||||
SG_ NEW_SIGNAL_12 : 179|4@0+ (1,0) [0|10] "" XXX
|
||||
SG_ NEW_SIGNAL_14 : 181|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_15 : 183|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ NEW_SIGNAL_16 : 185|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_17 : 187|2@0+ (1,0) [0|2] "" XXX
|
||||
|
||||
VAL_ {addr_dec} STATE_ALT 0 "EMPTY" 1 "TENTATIVE" 2 "MEASURED" 3 "COASTED" ;
|
||||
VAL_ {addr_dec} MOTION_STATE 0 "UNKNOWN" 1 "STATIONARY" 2 "MOVING" ;
|
||||
VAL_ {addr_dec} STATE 0 "EMPTY" 1 "TENTATIVE_1" 2 "TENTATIVE_2" 3 "MEASURED" 4 "COASTED" 7 "UNRESOLVED_TENTATIVE" ;
|
||||
"""
|
||||
|
||||
RADAR_3A5_3C4_DBC_TEMPLATE += "\n".join((
|
||||
'CM_ BO_ {addr_dec} "' + RADAR_3A5_3C4_MESSAGE_COMMENT + '";',
|
||||
*(f'CM_ SG_ {{addr_dec}} {signal} "{comment}";' for signal, comment in RADAR_3A5_3C4_SIGNAL_COMMENTS),
|
||||
)) + "\n"
|
||||
|
||||
RADAR_210_21F_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 32 RADAR
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ 1_AGE : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ 1_STATE_ALT : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 1_STATE : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 1_NEW_SIGNAL_3 : 63|8@0- (1,0) [0|255] "" XXX
|
||||
SG_ 1_LONG_DIST : 64|12@1+ (0.05,0) [0|4095] "" XXX
|
||||
SG_ 1_LAT_DIST : 76|12@1- (0.05,0) [0|4095] "" XXX
|
||||
SG_ 1_REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX
|
||||
SG_ 1_NEW_SIGNAL_1 : 102|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ 1_LAT_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX
|
||||
SG_ 1_REL_ACCEL : 118|10@1- (1,0) [0|1023] "" XXX
|
||||
SG_ 2_AGE : 175|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ 2_STATE_ALT : 179|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 2_STATE : 183|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 2_NEW_SIGNAL_3 : 191|8@0- (1,0) [0|255] "" XXX
|
||||
SG_ 2_LONG_DIST : 192|12@1+ (0.05,0) [0|4095] "" XXX
|
||||
SG_ 2_LAT_DIST : 204|12@1- (0.05,0) [0|4095] "" XXX
|
||||
SG_ 2_REL_SPEED : 216|14@1- (0.01,0) [0|65535] "" XXX
|
||||
SG_ 2_NEW_SIGNAL_1 : 230|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ 2_LAT_ACCEL : 232|13@1- (1,0) [0|8191] "" XXX
|
||||
SG_ 2_REL_ACCEL : 246|10@1- (1,0) [0|1023] "" XXX
|
||||
"""
|
||||
|
||||
RADAR_602_611_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
|
||||
SG_ 1_DISTANCE : 0|10@1+ (0.25,0) [0|255.75] "" XXX
|
||||
SG_ 1_LATERAL : 10|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX
|
||||
SG_ 1_SPEED : 21|10@1+ (0.25,-128) [-128|127.75] "" XXX
|
||||
SG_ 2_DISTANCE : 31|10@1+ (0.25,0) [0|255.75] "" XXX
|
||||
SG_ 2_LATERAL : 41|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX
|
||||
SG_ 2_SPEED : 52|8@1- (0.25,0) [-32|31.75] "" XXX
|
||||
SG_ UNKNOWN_2 : 60|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ COUNTER : 62|2@1+ (1,0) [0|3] "" XXX
|
||||
"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RadarSpec:
|
||||
name: str
|
||||
start_addr: int
|
||||
msg_count: int
|
||||
dbc_template: str
|
||||
track_prefixes: tuple[str, ...]
|
||||
frequency: int
|
||||
|
||||
@property
|
||||
def end_addr(self) -> int:
|
||||
return self.start_addr + self.msg_count - 1
|
||||
|
||||
def contains(self, address: int) -> bool:
|
||||
return self.start_addr <= address <= self.end_addr
|
||||
|
||||
|
||||
RADAR_SPECS = (
|
||||
RadarSpec("RADAR_500_51F", 0x500, 32, RADAR_500_51F_DBC_TEMPLATE, ("",), frequency=20),
|
||||
RadarSpec("RADAR_210_21F", 0x210, 16, RADAR_210_21F_DBC_TEMPLATE, ("1_", "2_"), frequency=20),
|
||||
RadarSpec("RADAR_3A5_3C4", 0x3A5, 32, RADAR_3A5_3C4_DBC_TEMPLATE, ("",), frequency=20),
|
||||
RadarSpec("RADAR_602_611", 0x602, 16, RADAR_602_611_DBC_TEMPLATE, ("1_", "2_"), frequency=10),
|
||||
)
|
||||
|
||||
|
||||
def build_seen_address_map() -> dict[str, set[int]]:
|
||||
return {radar_spec.name: set() for radar_spec in RADAR_SPECS}
|
||||
|
||||
|
||||
def get_radar_spec(address: int) -> RadarSpec | None:
|
||||
for radar_spec in RADAR_SPECS:
|
||||
if radar_spec.contains(address):
|
||||
return radar_spec
|
||||
return None
|
||||
|
||||
|
||||
def is_exclusive_full_range_match(radar_spec: RadarSpec, seen_addresses: dict[str, set[int]]) -> bool:
|
||||
expected_addresses = set(range(radar_spec.start_addr, radar_spec.end_addr + 1))
|
||||
if seen_addresses[radar_spec.name] != expected_addresses:
|
||||
return False
|
||||
|
||||
for other_spec in RADAR_SPECS:
|
||||
if other_spec.name == radar_spec.name:
|
||||
continue
|
||||
|
||||
other_expected_addresses = set(range(other_spec.start_addr, other_spec.end_addr + 1))
|
||||
if seen_addresses[other_spec.name] == other_expected_addresses:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
|
||||
def get_radar_dbc_path(radar_spec: RadarSpec) -> str:
|
||||
dbc_path = os.path.join(tempfile.gettempdir(), f"{radar_spec.name.lower()}_radar_ui.dbc")
|
||||
dbc_content = "\n".join(
|
||||
radar_spec.dbc_template.format(addr_dec=addr, addr_hex=f"{addr:x}")
|
||||
for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)
|
||||
)
|
||||
if not os.path.exists(dbc_path) or open(dbc_path).read() != dbc_content:
|
||||
with open(dbc_path, "w") as f:
|
||||
f.write(dbc_content)
|
||||
return dbc_path
|
||||
|
||||
|
||||
def get_radar_can_parser(radar_spec: RadarSpec, bus: int) -> CANParser:
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_spec.frequency) for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)]
|
||||
return CANParser(get_radar_dbc_path(radar_spec), messages, bus)
|
||||
|
||||
|
||||
def get_track_storage_key(radar_spec: RadarSpec, bus: int, addr: int, track_prefix: str) -> tuple[str, int, int]:
|
||||
if radar_spec.name in ("RADAR_500_51F", "RADAR_3A5_3C4"):
|
||||
return (radar_spec.name, bus, addr)
|
||||
|
||||
track_index = int(track_prefix[0]) - 1
|
||||
return (radar_spec.name, bus, addr * 2 + track_index)
|
||||
|
||||
|
||||
def get_track_ts_nanos(parser: CANParser, msg_name: str, radar_spec: RadarSpec, track_prefix: str) -> int:
|
||||
if radar_spec.name == "RADAR_602_611":
|
||||
return parser.ts_nanos[msg_name][f"{track_prefix}DISTANCE"]
|
||||
if radar_spec.name == "RADAR_210_21F":
|
||||
return parser.ts_nanos[msg_name][f"{track_prefix}LONG_DIST"]
|
||||
return parser.ts_nanos[msg_name]["LONG_DIST"]
|
||||
|
||||
|
||||
def decode_radar_track(radar_spec: RadarSpec, track_msg, track_prefix: str) -> tuple[float, float, float, float]:
|
||||
if radar_spec.name == "RADAR_602_611":
|
||||
return (
|
||||
track_msg[f"{track_prefix}DISTANCE"],
|
||||
track_msg[f"{track_prefix}LATERAL"],
|
||||
track_msg[f"{track_prefix}SPEED"],
|
||||
float("nan"),
|
||||
)
|
||||
|
||||
if radar_spec.name == "RADAR_210_21F":
|
||||
return (
|
||||
track_msg[f"{track_prefix}LONG_DIST"],
|
||||
track_msg[f"{track_prefix}LAT_DIST"],
|
||||
track_msg[f"{track_prefix}REL_SPEED"],
|
||||
float("nan"),
|
||||
)
|
||||
|
||||
if radar_spec.name == "RADAR_500_51F":
|
||||
azimuth = math.radians(track_msg["AZIMUTH"])
|
||||
long_dist = track_msg["LONG_DIST"]
|
||||
return (
|
||||
math.cos(azimuth) * long_dist,
|
||||
0.5 * -math.sin(azimuth) * long_dist,
|
||||
track_msg["REL_SPEED"],
|
||||
track_msg["REL_ACCEL"],
|
||||
)
|
||||
|
||||
return (
|
||||
track_msg["LONG_DIST"],
|
||||
track_msg["LAT_DIST"],
|
||||
track_msg["REL_SPEED"],
|
||||
track_msg["REL_ACCEL"],
|
||||
)
|
||||
|
||||
|
||||
def is_radar_track_valid(radar_spec: RadarSpec, track_msg, track_prefix: str) -> bool:
|
||||
if radar_spec.name == "RADAR_602_611":
|
||||
return 0 < track_msg[f"{track_prefix}DISTANCE"] < 255.75
|
||||
|
||||
if radar_spec.name == "RADAR_210_21F":
|
||||
return track_msg[f"{track_prefix}STATE"] in (3, 4)
|
||||
|
||||
return track_msg["STATE"] in (3, 4)
|
||||
Executable
+912
@@ -0,0 +1,912 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from opendbc.car.tests.routes import routes
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.tools.radar.custom_routes import CUSTOM_ROUTES
|
||||
from openpilot.tools.replay.lib.ui_helpers import (
|
||||
UP,
|
||||
BLACK,
|
||||
GREEN,
|
||||
Calibration,
|
||||
get_blank_lid_overlay,
|
||||
init_plots,
|
||||
plot_lead,
|
||||
plot_model,
|
||||
to_topdown_pt,
|
||||
)
|
||||
from openpilot.tools.radar.radar_helpers import (
|
||||
RADAR_SPECS,
|
||||
build_seen_address_map,
|
||||
decode_radar_track,
|
||||
get_radar_can_parser,
|
||||
get_radar_spec,
|
||||
get_track_storage_key,
|
||||
get_track_ts_nanos,
|
||||
is_exclusive_full_range_match,
|
||||
is_radar_track_valid,
|
||||
)
|
||||
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
ANGLE_SCALE = 5.0
|
||||
RADAR_TRACK_TIMEOUT_FRAMES = 10
|
||||
RADAR_FORMAT_SWITCH_MISS_FRAMES = 30
|
||||
RADAR_TRACK_RADIUS = 4
|
||||
CAMERA_RADAR_Y_OFFSET = 25
|
||||
RADAR_HEATMAP_DECAY = 0.975
|
||||
RADAR_HEATMAP_ALPHA = 0.65
|
||||
CAMERA_RADAR_HEATMAP_ALPHA = 0.45
|
||||
REPLAY_PATH = os.path.join(os.path.dirname(__file__), "..", "replay", "replay")
|
||||
REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS = 10.0
|
||||
REPLAY_SPEEDS = (0.2, 0.5, 1.0, 2.0, 4.0, 8.0)
|
||||
CAMERA_DRAW_WIDTH = 640
|
||||
CAMERA_DRAW_HEIGHT = 480
|
||||
TOP_DOWN_DRAW_WIDTH = 384
|
||||
PLOT_DRAW_WIDTH = 480
|
||||
PLOT_DRAW_HEIGHT = 480
|
||||
RADAR_HEATMAP_MODES = ("OFF", "TOP", "CAMERA", "BOTH")
|
||||
CYAN = (90, 235, 255)
|
||||
AMBER = (255, 210, 90)
|
||||
SOFT_WHITE = (235, 235, 235)
|
||||
SLATE = (140, 180, 210)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RadarTrackPoint:
|
||||
trackId: int
|
||||
measured: bool = True
|
||||
dRel: float = 0.0
|
||||
yRel: float = 0.0
|
||||
vRel: float = 0.0
|
||||
aRel: float = 0.0
|
||||
yvRel: float = float("nan")
|
||||
|
||||
|
||||
def draw_radar_points(tracks, lid_overlay):
|
||||
for track in tracks:
|
||||
px, py = to_topdown_pt(track.dRel, -track.yRel)
|
||||
if px != -1:
|
||||
cv2.circle(lid_overlay, (py, px), RADAR_TRACK_RADIUS, 255, thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def update_radar_heatmap(tracks, radar_heatmap):
|
||||
radar_heatmap *= RADAR_HEATMAP_DECAY
|
||||
for track in tracks:
|
||||
px, py = to_topdown_pt(track.dRel, -track.yRel)
|
||||
if px != -1:
|
||||
cv2.circle(radar_heatmap, (py, px), RADAR_TRACK_RADIUS + 2, 255.0, thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def update_radar_camera_heatmap(tracks, radar_heatmap, calibration, shape):
|
||||
radar_heatmap *= RADAR_HEATMAP_DECAY
|
||||
if calibration is None:
|
||||
return
|
||||
|
||||
height, width = shape[:2]
|
||||
for track in tracks:
|
||||
if track.dRel <= 0.0:
|
||||
continue
|
||||
|
||||
pt = calibration.car_space_to_bb(
|
||||
np.asarray([track.dRel - RADAR_TO_CAMERA]),
|
||||
np.asarray([-track.yRel]),
|
||||
np.asarray([1.0]),
|
||||
)
|
||||
x, y = np.round(pt[0]).astype(int)
|
||||
y += CAMERA_RADAR_Y_OFFSET
|
||||
if 0 <= x < width and 0 <= y < height:
|
||||
cv2.circle(radar_heatmap, (x, y), RADAR_TRACK_RADIUS + 3, 255.0, thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def overlay_heatmap(img, radar_heatmap, alpha_scale):
|
||||
radar_heat_uint8 = np.ascontiguousarray(np.clip(radar_heatmap, 0, 255).astype(np.uint8))
|
||||
radar_heat_mask = radar_heat_uint8 > 0
|
||||
if not np.any(radar_heat_mask):
|
||||
return
|
||||
|
||||
heat_rgb = cv2.cvtColor(cv2.applyColorMap(radar_heat_uint8, cv2.COLORMAP_TURBO), cv2.COLOR_BGR2RGB).astype(np.float32)
|
||||
img_rgb = img.astype(np.float32)
|
||||
alpha = ((radar_heat_uint8.astype(np.float32) / 255.0) * alpha_scale)[..., None]
|
||||
img[:] = np.where(
|
||||
radar_heat_mask[..., None],
|
||||
np.clip(img_rgb * (1.0 - alpha) + heat_rgb * alpha, 0, 255).astype(np.uint8),
|
||||
img,
|
||||
)
|
||||
|
||||
|
||||
def draw_radar_points_camera(tracks, img, calibration):
|
||||
if calibration is None:
|
||||
return
|
||||
|
||||
for track in tracks:
|
||||
if track.dRel <= 0.0:
|
||||
continue
|
||||
|
||||
# Match the road-space projection convention used by other UI overlays.
|
||||
pt = calibration.car_space_to_bb(
|
||||
np.asarray([track.dRel - RADAR_TO_CAMERA]),
|
||||
np.asarray([-track.yRel]),
|
||||
np.asarray([1.0]),
|
||||
)
|
||||
x, y = np.round(pt[0]).astype(int)
|
||||
y += CAMERA_RADAR_Y_OFFSET
|
||||
if 0 <= x < img.shape[1] and 0 <= y < img.shape[0]:
|
||||
cv2.circle(img, (x, y), RADAR_TRACK_RADIUS, (255, 255, 255), thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def draw_loading_overlay(font, lines, camera_texture, top_down_texture, hor_mode, panel_x, panel_y):
|
||||
rl.draw_texture_pro(
|
||||
camera_texture,
|
||||
rl.Rectangle(0, 0, camera_texture.width, camera_texture.height),
|
||||
rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT),
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
)
|
||||
rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251
|
||||
|
||||
if hor_mode:
|
||||
panel_width = 620
|
||||
panel_height = 300
|
||||
else:
|
||||
panel_width = 700
|
||||
panel_height = 320
|
||||
|
||||
rl.draw_rectangle(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(0, 0, 0, 140))
|
||||
rl.draw_rectangle_lines(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(255, 255, 255, 90))
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line:
|
||||
rl.draw_text_ex(font, line, rl.Vector2(panel_x, panel_y + i * 40), 28 if i == 0 else 20, 0, rl.WHITE)
|
||||
|
||||
|
||||
def start_replay(route: str, prefix: str, playback: str, data_dir: str | None, start_seconds: int) -> subprocess.Popen:
|
||||
cmd = [
|
||||
REPLAY_PATH,
|
||||
"--playback", playback,
|
||||
"--prefix", prefix,
|
||||
]
|
||||
if start_seconds > 0:
|
||||
cmd.extend(["--start", str(start_seconds)])
|
||||
if data_dir:
|
||||
cmd.extend(["--data_dir", data_dir])
|
||||
cmd.append(route)
|
||||
|
||||
env = os.environ.copy()
|
||||
env["OPENPILOT_PREFIX"] = prefix
|
||||
return subprocess.Popen(
|
||||
cmd,
|
||||
cwd=os.path.dirname(REPLAY_PATH),
|
||||
env=env,
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
preexec_fn=os.setsid if os.name != "nt" else None,
|
||||
)
|
||||
|
||||
|
||||
def stop_replay(proc: subprocess.Popen | None) -> None:
|
||||
if proc is not None and proc.poll() is None:
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
else:
|
||||
proc.terminate()
|
||||
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
else:
|
||||
proc.kill()
|
||||
proc.wait(timeout=5)
|
||||
|
||||
# `replay` can leave helper processes like `replayd` behind, so do one
|
||||
# targeted cleanup pass on shutdown as well.
|
||||
for pattern in ("/tools/replay/replay", "replayd"):
|
||||
try:
|
||||
subprocess.run(
|
||||
["pkill", "-f", pattern],
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
check=False,
|
||||
)
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
|
||||
|
||||
def wait_for_can_socket(prefix: str, timeout: float) -> None:
|
||||
socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can")
|
||||
started = time.monotonic()
|
||||
while not os.path.exists(socket_path):
|
||||
if time.monotonic() - started > timeout:
|
||||
raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}")
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def get_hkg_routes() -> list[tuple[str, str]]:
|
||||
hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_")
|
||||
return [
|
||||
(route.route, getattr(route.car_model, "name", str(route.car_model)))
|
||||
for route in routes
|
||||
if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes)
|
||||
]
|
||||
|
||||
|
||||
def get_custom_routes() -> list[tuple[str, str]]:
|
||||
return [(route.route, route.car_model) for route in CUSTOM_ROUTES]
|
||||
|
||||
|
||||
def make_submaster(addr):
|
||||
return messaging.SubMaster(
|
||||
[
|
||||
'carState',
|
||||
'longitudinalPlan',
|
||||
'carControl',
|
||||
'radarState',
|
||||
'liveCalibration',
|
||||
'controlsState',
|
||||
'selfdriveState',
|
||||
'liveTracks',
|
||||
'modelV2',
|
||||
'liveParameters',
|
||||
'roadCameraState',
|
||||
],
|
||||
addr=addr,
|
||||
)
|
||||
|
||||
|
||||
def reset_radar_state():
|
||||
return (
|
||||
0,
|
||||
None,
|
||||
0,
|
||||
{radar_spec.name: 0 for radar_spec in RADAR_SPECS},
|
||||
build_seen_address_map(),
|
||||
{},
|
||||
0,
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
)
|
||||
|
||||
|
||||
def ui_thread(addr, route_entries=None, playback="1.0", data_dir=None, prefix="ui-replay", start_route_idx=0):
|
||||
cv2.setNumThreads(1)
|
||||
|
||||
# Get monitor info before creating window
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
|
||||
rl.init_window(1, 1, "")
|
||||
max_height = rl.get_monitor_height(0)
|
||||
rl.close_window()
|
||||
|
||||
hor_mode = os.getenv("HORIZONTAL") is not None
|
||||
hor_mode = True if max_height < 960 + 300 else hor_mode
|
||||
|
||||
if hor_mode:
|
||||
size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH + PLOT_DRAW_WIDTH, 960)
|
||||
write_x = 5
|
||||
write_y = 480
|
||||
else:
|
||||
size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 960 + 300)
|
||||
write_x = CAMERA_DRAW_WIDTH + 5
|
||||
write_y = 970
|
||||
|
||||
rl.set_trace_log_level(rl.TraceLogLevel.LOG_ERROR)
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
|
||||
rl.init_window(size[0], size[1], "openpilot debug UI")
|
||||
rl.set_target_fps(60)
|
||||
|
||||
# Load font
|
||||
font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf")
|
||||
font = rl.load_font_ex(font_path, 32, None, 0)
|
||||
|
||||
# Create textures for camera and top-down view
|
||||
camera_image = rl.gen_image_color(640, 480, rl.BLACK)
|
||||
camera_texture = rl.load_texture_from_image(camera_image)
|
||||
rl.unload_image(camera_image)
|
||||
|
||||
# lid_overlay array is (lidar_x, lidar_y) = (384, 960)
|
||||
# pygame treats first axis as width, so texture is 384 wide x 960 tall
|
||||
# For raylib, we need to transpose to get (height, width) = (960, 384) for the RGBA array
|
||||
top_down_image = rl.gen_image_color(UP.lidar_x, UP.lidar_y, rl.BLACK)
|
||||
top_down_texture = rl.load_texture_from_image(top_down_image)
|
||||
rl.unload_image(top_down_image)
|
||||
|
||||
current_route_idx = start_route_idx
|
||||
current_route_name = None
|
||||
current_route_model = None
|
||||
replay_proc = None
|
||||
current_start_seconds = 0
|
||||
paused = False
|
||||
state_checks_enabled = False
|
||||
radar_heatmap_mode_idx = 0
|
||||
current_playback = min(REPLAY_SPEEDS, key=lambda speed: abs(speed - float(playback)))
|
||||
last_replay_started_at = time.monotonic()
|
||||
playback_ready = False
|
||||
loading_status = "Initializing UI"
|
||||
|
||||
def current_offset_seconds() -> int:
|
||||
if paused or not playback_ready or replay_proc is None or replay_proc.poll() is not None:
|
||||
return current_start_seconds
|
||||
return max(0, int(current_start_seconds + (time.monotonic() - last_replay_started_at) * current_playback))
|
||||
|
||||
def connect_streams():
|
||||
nonlocal replay_proc, current_route_name, current_route_model, current_route_idx, current_start_seconds, loading_status, paused, \
|
||||
last_replay_started_at, current_playback, playback_ready
|
||||
|
||||
if route_entries:
|
||||
current_route_name, current_route_model = route_entries[current_route_idx]
|
||||
loading_status = f"Starting replay for route {current_route_idx + 1}/{len(route_entries)} at {current_start_seconds}s ({current_playback:.1f}x)"
|
||||
stop_replay(replay_proc)
|
||||
os.environ["OPENPILOT_PREFIX"] = prefix
|
||||
messaging.reset_context()
|
||||
replay_proc = start_replay(current_route_name, prefix, f"{current_playback:.1f}", data_dir, current_start_seconds)
|
||||
paused = False
|
||||
playback_ready = False
|
||||
loading_status = "Waiting for CAN socket"
|
||||
wait_for_can_socket(prefix, REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS)
|
||||
|
||||
loading_status = "Connecting to messaging streams"
|
||||
sm_local = make_submaster(addr)
|
||||
logcan_local = messaging.sub_sock("can", addr=addr, conflate=False, timeout=100)
|
||||
loading_status = "Waiting for road camera frames"
|
||||
return sm_local, logcan_local
|
||||
|
||||
sm, logcan = connect_streams()
|
||||
|
||||
img = np.zeros((480, 640, 3), dtype='uint8')
|
||||
imgff = None
|
||||
num_px = 0
|
||||
calibration = None
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
|
||||
lid_overlay_blank = get_blank_lid_overlay(UP)
|
||||
radar_heatmap = np.zeros_like(lid_overlay_blank, dtype=np.float32)
|
||||
camera_radar_heatmap = np.zeros(img.shape[:2], dtype=np.float32)
|
||||
|
||||
# plots
|
||||
name_to_arr_idx = {
|
||||
"gas": 0,
|
||||
"computer_gas": 1,
|
||||
"user_brake": 2,
|
||||
"computer_brake": 3,
|
||||
"v_ego": 4,
|
||||
"v_pid": 5,
|
||||
"angle_steers_des": 6,
|
||||
"angle_steers": 7,
|
||||
"angle_steers_k": 8,
|
||||
"steer_torque": 9,
|
||||
"v_override": 10,
|
||||
"v_cruise": 11,
|
||||
"a_ego": 12,
|
||||
"a_target": 13,
|
||||
}
|
||||
|
||||
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
|
||||
|
||||
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
|
||||
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.0, 2.0)]
|
||||
plot_names = [
|
||||
["gas", "computer_gas", "user_brake", "computer_brake"],
|
||||
["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
|
||||
["v_ego", "v_override", "v_pid", "v_cruise"],
|
||||
["a_ego", "a_target"],
|
||||
]
|
||||
plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"], ["b", "g", "r", "y"], ["b", "r"]]
|
||||
plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]]
|
||||
|
||||
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)
|
||||
|
||||
# Palette for converting lid_overlay grayscale indices to RGBA colors
|
||||
palette = np.zeros((256, 4), dtype=np.uint8)
|
||||
palette[:, 3] = 255 # alpha
|
||||
palette[1] = [255, 0, 0, 255] # RED
|
||||
palette[2] = [0, 255, 0, 255] # GREEN
|
||||
palette[3] = [0, 0, 255, 255] # BLUE
|
||||
palette[4] = [255, 255, 0, 255] # YELLOW
|
||||
palette[110] = [110, 110, 110, 255] # car_color (gray)
|
||||
palette[255] = [255, 255, 255, 255] # WHITE
|
||||
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
while not rl.window_should_close():
|
||||
# ***** frame *****
|
||||
if not vipc_client.is_connected():
|
||||
vipc_client.connect(False)
|
||||
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(rl.Color(64, 64, 64, 255))
|
||||
|
||||
if rl.is_key_released(rl.KeyboardKey.KEY_Q):
|
||||
rl.end_drawing()
|
||||
stop_replay(replay_proc)
|
||||
replay_proc = None
|
||||
break
|
||||
|
||||
shift_down = rl.is_key_down(rl.KeyboardKey.KEY_LEFT_SHIFT) or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_SHIFT)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_SPACE):
|
||||
if paused:
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
else:
|
||||
current_start_seconds = current_offset_seconds()
|
||||
paused = True
|
||||
loading_status = "Paused"
|
||||
stop_replay(replay_proc)
|
||||
replay_proc = None
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_RIGHT):
|
||||
current_route_idx = (current_route_idx + 1) % len(route_entries)
|
||||
current_start_seconds = 0
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_LEFT):
|
||||
current_route_idx = (current_route_idx - 1) % len(route_entries)
|
||||
current_start_seconds = 0
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_M):
|
||||
current_start_seconds = max(0, current_offset_seconds() + (-60 if shift_down else 60))
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_S):
|
||||
current_start_seconds = max(0, current_offset_seconds() + (-10 if shift_down else 10))
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_EQUAL) or rl.is_key_released(rl.KeyboardKey.KEY_KP_ADD)):
|
||||
for speed in REPLAY_SPEEDS:
|
||||
if speed > current_playback:
|
||||
current_playback = speed
|
||||
break
|
||||
if not paused:
|
||||
current_start_seconds = current_offset_seconds()
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_MINUS) or rl.is_key_released(rl.KeyboardKey.KEY_KP_SUBTRACT)):
|
||||
for speed in reversed(REPLAY_SPEEDS):
|
||||
if speed < current_playback:
|
||||
current_playback = speed
|
||||
break
|
||||
if not paused:
|
||||
current_start_seconds = current_offset_seconds()
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if rl.is_key_released(rl.KeyboardKey.KEY_C):
|
||||
state_checks_enabled = not state_checks_enabled
|
||||
|
||||
if rl.is_key_released(rl.KeyboardKey.KEY_H):
|
||||
radar_heatmap_mode_idx = (radar_heatmap_mode_idx + 1) % len(RADAR_HEATMAP_MODES)
|
||||
if RADAR_HEATMAP_MODES[radar_heatmap_mode_idx] == "OFF":
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
|
||||
yuv_img_raw = vipc_client.recv()
|
||||
if yuv_img_raw is None or not yuv_img_raw.data.any():
|
||||
if replay_proc is not None and replay_proc.poll() is not None:
|
||||
loading_status = f"Replay exited with code {replay_proc.poll()}"
|
||||
|
||||
loading_lines = [
|
||||
f"{loading_status}{' (paused)' if paused else ''}",
|
||||
f"Route {current_route_idx + 1}/{len(route_entries)}" if route_entries else "Connected to external replay",
|
||||
f"Platform: {current_route_model}" if current_route_model is not None else "",
|
||||
f"Route: {current_route_name}" if current_route_name is not None else "",
|
||||
f"Offset: {current_offset_seconds()}s" if route_entries else "",
|
||||
f"Playback: {current_playback:.1f}x" if route_entries else "",
|
||||
f"Radar state checks: {'ON' if state_checks_enabled else 'OFF'}",
|
||||
f"Radar heatmap: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}",
|
||||
"Keys: SPACE play/pause, RIGHT next, LEFT prev, M +/-60s, S +/-10s, +/- speed, C checks, H heatmap, Q quit" \
|
||||
if route_entries else "Keys: C checks, H heatmap, Q quit",
|
||||
]
|
||||
draw_loading_overlay(font, loading_lines, camera_texture, top_down_texture, hor_mode, 80, 160)
|
||||
rl.end_drawing()
|
||||
continue
|
||||
|
||||
if not playback_ready:
|
||||
playback_ready = True
|
||||
last_replay_started_at = time.monotonic()
|
||||
loading_status = "Streaming"
|
||||
|
||||
lid_overlay = lid_overlay_blank.copy()
|
||||
top_down = top_down_texture, lid_overlay
|
||||
|
||||
sm.update(0)
|
||||
|
||||
camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
|
||||
|
||||
# Use received buffer dimensions (full HEVC can have stride != buffer_len/rows due to VENUS padding)
|
||||
h, w, stride = yuv_img_raw.height, yuv_img_raw.width, yuv_img_raw.stride
|
||||
nv12_size = h * 3 // 2 * stride
|
||||
imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8, count=nv12_size).reshape((h * 3 // 2, stride))
|
||||
num_px = w * h
|
||||
rgb = cv2.cvtColor(imgff[: h * 3 // 2, : w], cv2.COLOR_YUV2RGB_NV12)
|
||||
|
||||
qcam = "QCAM" in os.environ
|
||||
bb_scale = 0.825 if qcam else 0.8
|
||||
calib_scale = camera.fcam.width / 640.0
|
||||
zoom_matrix = np.asarray([[bb_scale, 0.0, 0.0], [0.0, bb_scale, 0.0], [0.0, 0.0, 1.0]])
|
||||
cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
||||
|
||||
intrinsic_matrix = camera.fcam.intrinsics
|
||||
|
||||
w = sm['controlsState'].lateralControlState.which()
|
||||
if w == 'lqrStateDEPRECATED':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg
|
||||
elif w == 'indiState':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
|
||||
else:
|
||||
angle_steers_k = np.inf
|
||||
|
||||
plot_arr[:-1] = plot_arr[1:]
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
|
||||
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gasDEPRECATED
|
||||
# TODO gas is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_gas']] = np.clip(sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
|
||||
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.torque * ANGLE_SCALE
|
||||
# TODO brake is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = np.clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
|
||||
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
|
||||
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
|
||||
|
||||
if len(sm['longitudinalPlan'].accels):
|
||||
plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]
|
||||
|
||||
if sm.recv_frame['modelV2']:
|
||||
plot_model(sm['modelV2'], img, calibration, top_down)
|
||||
|
||||
if sm.recv_frame['radarState']:
|
||||
plot_lead(sm['radarState'], top_down)
|
||||
|
||||
if sm.updated['liveCalibration'] and num_px:
|
||||
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
|
||||
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
|
||||
|
||||
can_packets = messaging.drain_sock(logcan)
|
||||
if can_packets:
|
||||
can_strings = [
|
||||
(can_packet.logMonoTime, [(msg.address, msg.dat, msg.src) for msg in can_packet.can])
|
||||
for can_packet in can_packets
|
||||
]
|
||||
detected_format_counts = {radar_spec.name: 0 for radar_spec in RADAR_SPECS}
|
||||
|
||||
for can_packet in can_packets:
|
||||
for msg in can_packet.can:
|
||||
radar_spec = get_radar_spec(msg.address)
|
||||
if radar_spec is not None:
|
||||
can_range_msg_count += 1
|
||||
detected_format_counts[radar_spec.name] += 1
|
||||
radar_format_total_counts[radar_spec.name] += 1
|
||||
radar_format_seen_addresses[radar_spec.name].add(msg.address)
|
||||
if radar_spec.name not in radar_parsers:
|
||||
radar_parsers[radar_spec.name] = {}
|
||||
if msg.src not in radar_parsers[radar_spec.name]:
|
||||
radar_parsers[radar_spec.name][msg.src] = get_radar_can_parser(radar_spec, msg.src)
|
||||
|
||||
matching_formats = [
|
||||
radar_spec.name
|
||||
for radar_spec in RADAR_SPECS
|
||||
if is_exclusive_full_range_match(radar_spec, radar_format_seen_addresses)
|
||||
]
|
||||
if len(matching_formats) == 1:
|
||||
if active_radar_format_name == matching_formats[0]:
|
||||
active_radar_format_miss_count = 0
|
||||
elif active_radar_format_name is None:
|
||||
active_radar_format_name = matching_formats[0]
|
||||
active_radar_format_miss_count = 0
|
||||
else:
|
||||
active_radar_format_miss_count += 1
|
||||
if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES:
|
||||
active_radar_format_name = matching_formats[0]
|
||||
active_radar_format_miss_count = 0
|
||||
elif len(matching_formats) == 0 and active_radar_format_name is not None:
|
||||
active_radar_format_miss_count += 1
|
||||
if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES:
|
||||
active_radar_format_name = None
|
||||
active_radar_format_miss_count = 0
|
||||
|
||||
active_radar_spec = next((spec for spec in RADAR_SPECS if spec.name == active_radar_format_name), None)
|
||||
if active_radar_spec is not None:
|
||||
for bus, parser in radar_parsers.get(active_radar_spec.name, {}).items():
|
||||
updated_addrs = parser.update(can_strings)
|
||||
relevant_updated_addrs = {
|
||||
track_addr for track_addr in updated_addrs
|
||||
if active_radar_spec.start_addr <= track_addr <= active_radar_spec.end_addr
|
||||
}
|
||||
if not relevant_updated_addrs:
|
||||
continue
|
||||
|
||||
for track_addr in relevant_updated_addrs:
|
||||
msg_name = f"RADAR_TRACK_{track_addr:x}"
|
||||
track_msg = parser.vl[msg_name]
|
||||
for track_prefix in active_radar_spec.track_prefixes:
|
||||
track_key = get_track_storage_key(active_radar_spec, bus, track_addr, track_prefix)
|
||||
ts_nanos = get_track_ts_nanos(parser, msg_name, active_radar_spec, track_prefix)
|
||||
if ts_nanos == 0:
|
||||
continue
|
||||
|
||||
if state_checks_enabled and not is_radar_track_valid(active_radar_spec, track_msg, track_prefix):
|
||||
radar_tracks.pop(track_key, None)
|
||||
radar_track_last_seen.pop(track_key, None)
|
||||
continue
|
||||
|
||||
d_rel, y_rel, v_rel, a_rel = decode_radar_track(active_radar_spec, track_msg, track_prefix)
|
||||
|
||||
if track_key not in radar_track_ids:
|
||||
radar_track_ids[track_key] = next_radar_track_id
|
||||
next_radar_track_id += 1
|
||||
|
||||
radar_tracks[track_key] = RadarTrackPoint(
|
||||
trackId=radar_track_ids[track_key],
|
||||
dRel=d_rel,
|
||||
yRel=y_rel,
|
||||
vRel=v_rel,
|
||||
aRel=a_rel,
|
||||
)
|
||||
radar_track_last_seen[track_key] = sm.frame
|
||||
|
||||
stale_tracks = [
|
||||
track_key for track_key, last_seen in radar_track_last_seen.items()
|
||||
if (sm.frame - last_seen) > RADAR_TRACK_TIMEOUT_FRAMES
|
||||
]
|
||||
for track_key in stale_tracks:
|
||||
radar_track_last_seen.pop(track_key, None)
|
||||
radar_tracks.pop(track_key, None)
|
||||
|
||||
active_radar_tracks = [
|
||||
track for track_key, track in radar_tracks.items()
|
||||
if active_radar_format_name is not None and track_key[0] == active_radar_format_name
|
||||
]
|
||||
active_radar_buses = sorted({
|
||||
track_key[1] for track_key in radar_tracks
|
||||
if active_radar_format_name is not None and track_key[0] == active_radar_format_name
|
||||
})
|
||||
if len(active_radar_tracks) == 0:
|
||||
active_radar_tracks = sm['liveTracks'].points
|
||||
active_radar_buses = []
|
||||
|
||||
radar_heatmap_mode = RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]
|
||||
if radar_heatmap_mode in ("TOP", "BOTH"):
|
||||
update_radar_heatmap(active_radar_tracks, radar_heatmap)
|
||||
if radar_heatmap_mode in ("CAMERA", "BOTH"):
|
||||
update_radar_camera_heatmap(active_radar_tracks, camera_radar_heatmap, calibration, img.shape)
|
||||
overlay_heatmap(img, camera_radar_heatmap, CAMERA_RADAR_HEATMAP_ALPHA)
|
||||
|
||||
# draw decoded radar tracks when present, otherwise fall back to liveTracks
|
||||
draw_radar_points(active_radar_tracks, top_down[1])
|
||||
draw_radar_points_camera(active_radar_tracks, img, calibration)
|
||||
|
||||
# *** blits ***
|
||||
# Update camera texture from numpy array
|
||||
img_rgba = cv2.cvtColor(img, cv2.COLOR_RGB2RGBA)
|
||||
rl.update_texture(camera_texture, rl.ffi.cast("void *", img_rgba.ctypes.data))
|
||||
rl.draw_texture_pro(
|
||||
camera_texture,
|
||||
rl.Rectangle(0, 0, camera_texture.width, camera_texture.height),
|
||||
rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT),
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
# display alerts
|
||||
rl.draw_text_ex(font, sm['selfdriveState'].alertText1, rl.Vector2(180, 150), 30, 0, rl.RED)
|
||||
rl.draw_text_ex(font, sm['selfdriveState'].alertText2, rl.Vector2(180, 190), 20, 0, rl.RED)
|
||||
|
||||
# draw plots (texture is reused internally)
|
||||
plot_texture = draw_plots(plot_arr)
|
||||
if hor_mode:
|
||||
rl.draw_texture_pro(
|
||||
plot_texture,
|
||||
rl.Rectangle(0, 0, plot_texture.width, plot_texture.height),
|
||||
rl.Rectangle(CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 0, PLOT_DRAW_WIDTH, PLOT_DRAW_HEIGHT),
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
)
|
||||
else:
|
||||
rl.draw_texture(plot_texture, 0, 300, rl.WHITE) # noqa: TID251
|
||||
|
||||
# Convert lid_overlay to RGBA and update top_down texture
|
||||
# lid_overlay is (384, 960), need to transpose to (960, 384) for row-major RGBA buffer
|
||||
lid_rgba = palette[lid_overlay.T]
|
||||
if radar_heatmap_mode in ("TOP", "BOTH"):
|
||||
overlay_heatmap(lid_rgba[..., :3], radar_heatmap.T, RADAR_HEATMAP_ALPHA)
|
||||
rl.update_texture(top_down_texture, rl.ffi.cast("void *", np.ascontiguousarray(lid_rgba).ctypes.data))
|
||||
rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251
|
||||
|
||||
SPACING = 20
|
||||
lines = [
|
||||
("ENABLED", GREEN if sm['selfdriveState'].enabled else BLACK),
|
||||
("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", CYAN),
|
||||
("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), CYAN),
|
||||
("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), CYAN),
|
||||
(f"RADAR FORMAT: {active_radar_format_name or 'NONE'}", AMBER),
|
||||
(f"RADAR CAN MSGS: {can_range_msg_count}", AMBER),
|
||||
(f"RADAR TRACKS: {len(active_radar_tracks)}"
|
||||
+ (f" (BUS {','.join(str(bus) for bus in active_radar_buses)})" if active_radar_buses else ""),
|
||||
AMBER),
|
||||
(f"RADAR STATE CHECKS: {'ON' if state_checks_enabled else 'OFF'}", AMBER),
|
||||
(f"RADAR HEATMAP: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}", AMBER),
|
||||
(f"ROUTE: {current_route_name}" if current_route_name is not None else "", SLATE),
|
||||
(f"PLATFORM: {current_route_model}" if current_route_model is not None else "", SLATE),
|
||||
(f"OFFSET: {current_offset_seconds()}s" if route_entries else "", SOFT_WHITE),
|
||||
(f"PLAYBACK: {current_playback:.1f}x" if route_entries else "", SOFT_WHITE),
|
||||
(f"STATUS: {'PAUSED' if paused else 'PLAYING'}" if route_entries else "", SOFT_WHITE),
|
||||
("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", SOFT_WHITE),
|
||||
("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", SOFT_WHITE),
|
||||
("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", SOFT_WHITE),
|
||||
("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), SOFT_WHITE),
|
||||
]
|
||||
|
||||
hud_height = len(lines) * SPACING + 18
|
||||
rl.draw_rectangle(write_x - 10, write_y - 12, 560, hud_height, rl.Color(8, 12, 18, 155))
|
||||
rl.draw_rectangle(write_x - 10, write_y - 12, 4, hud_height, rl.Color(90, 235, 255, 255))
|
||||
rl.draw_rectangle_lines(write_x - 10, write_y - 12, 560, hud_height, rl.Color(90, 235, 255, 80))
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line is not None:
|
||||
color = rl.Color(line[1][0], line[1][1], line[1][2], 255)
|
||||
rl.draw_text_ex(font, line[0], rl.Vector2(write_x, write_y + i * SPACING), 20, 0, color)
|
||||
|
||||
rl.end_drawing()
|
||||
|
||||
rl.unload_texture(camera_texture)
|
||||
rl.unload_texture(top_down_texture)
|
||||
rl.unload_font(font)
|
||||
rl.close_window()
|
||||
stop_replay(replay_proc)
|
||||
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(description="Show replay data in a UI.", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1", help="The ip address on which to receive zmq messages.")
|
||||
parser.add_argument("--route", default=None, help="Route to replay locally before opening the UI.")
|
||||
parser.add_argument("--routes", action="store_true", help="Cycle Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
|
||||
parser.add_argument("--custom-routes", nargs="?", type=int, const=1, default=None,
|
||||
help="Cycle routes from tools/radar/custom_routes.py, optionally starting from a 1-based route index.")
|
||||
parser.add_argument("--data-dir", default=None, help="Optional local route data directory to pass to replay.")
|
||||
parser.add_argument("--playback", default="1.0", help="Replay playback speed when using --route.")
|
||||
parser.add_argument("--prefix", default="ui-replay", help="OPENPILOT_PREFIX to use when launching replay from the UI.")
|
||||
parser.add_argument("--frame-address", default=None, help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
|
||||
selected_sources = int(args.route is not None) + int(args.routes) + int(args.custom_routes is not None)
|
||||
if selected_sources > 1:
|
||||
raise SystemExit("Use only one of --route, --routes, or --custom-routes.")
|
||||
|
||||
route_entries = None
|
||||
start_route_idx = 0
|
||||
if args.route is not None:
|
||||
route_entries = [(args.route, "MANUAL_ROUTE")]
|
||||
elif args.routes:
|
||||
route_entries = get_hkg_routes()
|
||||
elif args.custom_routes is not None:
|
||||
route_entries = get_custom_routes()
|
||||
start_route_idx = max(0, min(len(route_entries) - 1, args.custom_routes - 1))
|
||||
|
||||
if route_entries:
|
||||
os.environ["OPENPILOT_PREFIX"] = args.prefix
|
||||
messaging.reset_context()
|
||||
elif args.ip_address != "127.0.0.1":
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
|
||||
ui_thread(args.ip_address, route_entries=route_entries, playback=args.playback, data_dir=args.data_dir, prefix=args.prefix, start_route_idx=start_route_idx)
|
||||
Executable
+2959
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user