version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
75 lines
2.8 KiB
Python
75 lines
2.8 KiB
Python
"""
|
|
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 opendbc.car import uds
|
|
from opendbc.car.carlog import carlog
|
|
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
|
|
|
|
DEVELOPER_DIAGNOSTIC = 0x07
|
|
CUSTOM_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, DEVELOPER_DIAGNOSTIC])
|
|
CUSTOM_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, DEVELOPER_DIAGNOSTIC])
|
|
|
|
READ_DATA_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER])
|
|
READ_DATA_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
|
|
|
|
WRITE_DATA_REQUEST = bytes([uds.SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER])
|
|
WRITE_DATA_RESPONSE = bytes([uds.SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER + 0x40])
|
|
|
|
CONFIG_DATA_ID = bytes([0x01, 0x42])
|
|
DEFAULT_CONFIG = bytes([0x00, 0x00, 0x00, 0x01, 0x00, 0x00])
|
|
TRACKS_ENABLED_CONFIG = bytes([0x00, 0x00, 0x00, 0x01, 0x00, 0x01])
|
|
TRACKS_ENABLED_CONFIG_BYTES = b"\x00\x00\x01\x00\x01"
|
|
|
|
|
|
def enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, timeout=0.1, retry=2):
|
|
carlog.error("radar_tracks: enabling ...")
|
|
|
|
for i in range(retry):
|
|
try:
|
|
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [CUSTOM_DIAGNOSTIC_REQUEST], [CUSTOM_DIAGNOSTIC_RESPONSE])
|
|
|
|
for _, _ in query.get_data(timeout).items():
|
|
carlog.error("radar_tracks: check current config ...")
|
|
|
|
request = READ_DATA_REQUEST + CONFIG_DATA_ID
|
|
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [request], [READ_DATA_RESPONSE])
|
|
|
|
for _, data in query.get_data(timeout).items():
|
|
current_config = data[3:]
|
|
|
|
carlog.error(f"radar_tracks: current config: {current_config.hex()}")
|
|
|
|
if current_config == TRACKS_ENABLED_CONFIG_BYTES:
|
|
carlog.error("radar_tracks: already enabled, skipping ...")
|
|
else:
|
|
carlog.error("radar_tracks: reconfigure radar to output radar points ...")
|
|
request = WRITE_DATA_REQUEST + CONFIG_DATA_ID + TRACKS_ENABLED_CONFIG
|
|
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [request], [WRITE_DATA_RESPONSE])
|
|
query.get_data(0)
|
|
|
|
carlog.error("radar_tracks: successfully enabled")
|
|
|
|
return True
|
|
|
|
except Exception as e:
|
|
carlog.exception(f"radar_tracks exception: {e}")
|
|
|
|
carlog.error(f"radar_tracks retry ({i + 1}) ...")
|
|
carlog.error("radar_tracks: failed")
|
|
return False
|
|
|
|
|
|
if __name__ == "__main__":
|
|
import time
|
|
import openpilot.cereal.messaging as messaging
|
|
sendcan = messaging.pub_sock('sendcan')
|
|
logcan = messaging.sub_sock('can')
|
|
time.sleep(1)
|
|
|
|
enabled = enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, timeout=0.1)
|
|
print(f"enabled: {enabled}")
|