mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-30 08:23:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

freq

Browse Source
This commit is contained in:
royjr
2026-07-16 10:18:00 -04:00
parent 1e8d6bfc48
commit a33cdaef87
1 changed files with 46 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/radar/radar_helpers.py
+46 -23
View File
@@ -5,6 +5,7 @@ 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 = """
@@ -25,28 +26,48 @@ 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_ NEW_SIGNAL_1 : 25|2@0+ (1,0) [0|3] "" XXX
SG_ NEW_SIGNAL_3 : 28|2@0+ (1,0) [0|3] "" XXX
SG_ COUNTER_3 : 31|2@0+ (1,0) [0|3] "" XXX
SG_ NEW_SIGNAL_2 : 38|7@0- (1,0) [0|127] "" XXX
SG_ COUNTER_256 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_6 : 51|4@0+ (1,0) [0|15] "" 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_ NEW_SIGNAL_2 : 38|7@0- (1,0) [-64|63] "" 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_ NEW_SIGNAL_8 : 62|7@0- (1,0) [0|127] "" XXX
SG_ LONG_DIST : 63|12@1+ (0.05,0) [0|8191] "m" XXX
SG_ LAT_DIST : 76|12@1- (0.05,0) [0|127] "" XXX
SG_ REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX
SG_ NEW_SIGNAL_4 : 103|2@0+ (1,0) [0|3] "" XXX
SG_ LAT_DIST_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX
SG_ REL_ACCEL : 118|10@1- (0.02,0) [0|1023] "" XXX
SG_ NEW_SIGNAL_5 : 133|4@0+ (1,0) [0|15] "" XXX
SG_ NEW_SIGNAL_8 : 62|7@0- (1,0) [-33|37] "" 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_COUNTER_255 : 47|8@0+ (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
@@ -56,7 +77,7 @@ BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 32 RADAR
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_COUNTER_255 : 175|8@0+ (1,0) [0|255] "" 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
@@ -75,7 +96,8 @@ BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
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|10@1+ (0.25,-128) [-128|127.75] "" 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
"""
@@ -87,6 +109,7 @@ class RadarSpec:
msg_count: int
dbc_template: str
track_prefixes: tuple[str, ...]
frequency: int
@property
def end_addr(self) -> int:
@@ -97,10 +120,10 @@ class RadarSpec:
RADAR_SPECS = (
RadarSpec("RADAR_500_51F", 0x500, 32, RADAR_500_51F_DBC_TEMPLATE, ("",)),
RadarSpec("RADAR_210_21F", 0x210, 16, RADAR_210_21F_DBC_TEMPLATE, ("1_", "2_")),
RadarSpec("RADAR_3A5_3C4", 0x3A5, 32, RADAR_3A5_3C4_DBC_TEMPLATE, ("",)),
RadarSpec("RADAR_602_611", 0x602, 16, RADAR_602_611_DBC_TEMPLATE, ("1_", "2_")),
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),
)
@@ -144,7 +167,7 @@ def get_radar_dbc_path(radar_spec: RadarSpec) -> str:
def get_radar_can_parser(radar_spec: RadarSpec, bus: int) -> CANParser:
messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)]
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)
@@ -201,7 +224,7 @@ def decode_radar_track(radar_spec: RadarSpec, track_msg, track_prefix: str) -> t
def is_radar_track_valid(radar_spec: RadarSpec, track_msg, track_prefix: str) -> bool:
if radar_spec.name == "RADAR_602_611":
return track_msg[f"{track_prefix}DISTANCE"] != 255.75
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)