72 lines
2.7 KiB
Python
72 lines
2.7 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
|
|
from iqdbc.car import uds
|
|
from iqdbc.car.carlog import carlog
|
|
from iqdbc.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 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}")
|