4D 毫米波雷达读取数据篇（一）
发布时间
阅读量：
阅读量
1、硬件：
Xavier NX系列芯片中采用Linux内核版本的CanFD收发器盒，在满足功能需求的同时提升了整体系统的性能表现。纳雷科技推出了SR-75 4D毫米波雷达系统（支持高达4M波特率的数据传输协议），该方案特别适用于复杂环境下的高精度目标检测与跟踪任务。该技术方案主要针对现有Can总线协议在高波特率应用中的技术局限性进行了优化改进
windows系统使用官方的软件读取到的雷达点云：
linux系统canfd读取到的点云，用python绘制：
2、canfd代码部分：
库文件：libcontrolcanfd.so
注意事项：在本地Linux系统环境下需进行厂家特定的交叉编译操作才能正常运行
#
#python3.8.8 64位（python 32位要用32位的DLL）
#
# V2.0 -- add CAN frame send and recv. add close dev. add filter.
#
from ctypes import *
import time
from unittest import result
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
VCI_USBCAN2 = 41
STATUS_OK = 1
INVALID_DEVICE_HANDLE = 0
INVALID_CHANNEL_HANDLE = 0
TYPE_CAN = 0
TYPE_CANFD = 1
class VCI_INIT_CONFIG(Structure):
_fields_ = [("AccCode", c_uint),
("AccMask", c_uint),
("Reserved", c_uint),
("Filter", c_ubyte),
("Timing0", c_ubyte),
("Timing1", c_ubyte),
("Mode", c_ubyte)
]
class VCI_CAN_OBJ(Structure):
_fields_ = [("ID", c_uint),
("TimeStamp", c_uint),
("TimeFlag", c_ubyte),
("SendType", c_ubyte),
("RemoteFlag", c_ubyte),
("ExternFlag", c_ubyte),
("DataLen", c_ubyte),
("Data", c_ubyte*8),
("Reserved", c_ubyte*3)
]
### structure
class _ZCAN_CHANNEL_CAN_INIT_CONFIG(Structure):
_fields_ = [("acc_code", c_uint),
("acc_mask", c_uint),
("reserved", c_uint),
("filter", c_ubyte),
("timing0", c_ubyte),
("timing1", c_ubyte),
("mode", c_ubyte)]
class _ZCAN_CHANNEL_CANFD_INIT_CONFIG(Structure):
_fields_ = [("acc_code", c_uint),
("acc_mask", c_uint),
("abit_timing", c_uint),
("dbit_timing", c_uint),
("brp", c_uint),
("filter", c_ubyte),
("mode", c_ubyte),
("pad", c_ushort),
("reserved", c_uint)]
class _ZCAN_CHANNEL_INIT_CONFIG(Union):
_fields_ = [("can", _ZCAN_CHANNEL_CAN_INIT_CONFIG), ("canfd", _ZCAN_CHANNEL_CANFD_INIT_CONFIG)]
class ZCAN_CHANNEL_INIT_CONFIG(Structure):
_fields_ = [("can_type", c_uint),
("config", _ZCAN_CHANNEL_INIT_CONFIG)]
class ZCAN_CAN_FRAME(Structure):
_fields_ = [("can_id", c_uint, 29),
("err", c_uint, 1),
("rtr", c_uint, 1),
("eff", c_uint, 1),
("can_dlc", c_ubyte),
("__pad", c_ubyte),
("__res0", c_ubyte),
("__res1", c_ubyte),
("data", c_ubyte * 8)]
class ZCAN_CANFD_FRAME(Structure):
_fields_ = [("can_id", c_uint, 29),
("err", c_uint, 1),
("rtr", c_uint, 1),
("eff", c_uint, 1),
("len", c_ubyte),
("brs", c_ubyte, 1),
("esi", c_ubyte, 1),
("__res", c_ubyte, 6),
("__res0", c_ubyte),
("__res1", c_ubyte),
("data", c_ubyte * 64)]
class ZCAN_Transmit_Data(Structure):
_fields_ = [("frame", ZCAN_CAN_FRAME), ("transmit_type", c_uint)]
class ZCAN_Receive_Data(Structure):
_fields_ = [("frame", ZCAN_CAN_FRAME), ("timestamp", c_ulonglong)]
class ZCAN_TransmitFD_Data(Structure):
_fields_ = [("frame", ZCAN_CANFD_FRAME), ("transmit_type", c_uint)]
class ZCAN_ReceiveFD_Data(Structure):
_fields_ = [("frame", ZCAN_CANFD_FRAME), ("timestamp", c_ulonglong)]
# 转换的函数
def PointCloundtoXY(chunk):
# 将字符串转换为整数
# 去除空格并将十六进制字符串转换为整数
# print("chunck****************: ",chunk)
dis_y = ((chunk[1]*32)+(chunk[2]>>3))*0.05-100 # m
dis_x = ((chunk[2]&0x07)*256+(chunk[3]))*0.05-50 # m
vel_y = ((chunk[4]*4)+(chunk[5]>>6))*0.05-16 # m/s
dis_z = (((chunk[5]&0x3F)*8)+(chunk[6]>>5))*0.25-64 # m
point_xy = [round(dis_x, 2),round(dis_y, 2),round(dis_z, 2),round(vel_y, 2)]
return point_xy
CanDLLName = './libcontrolcanfd.so' #把DLL放到对应的目录下
# canDLL = windll.LoadLibrary('./libcontrolcanfd.so')
#Linux系统下使用下面语句，编译命令：python3 cxcanfd.py
canDLL = cdll.LoadLibrary('./libcontrolcanfd.so')
print('########################################################')
print('## Chuang Xin USBCANFD python(x64) test program V2.0 ###')
print('########################################################')
print(CanDLLName)
canDLL.ZCAN_OpenDevice.restype = c_void_p
canDLL.ZCAN_SetAbitBaud.argtypes = (c_void_p, c_ulong, c_ulong)
canDLL.ZCAN_SetDbitBaud.argtypes = (c_void_p, c_ulong, c_ulong)
canDLL.ZCAN_SetCANFDStandard.argtypes = (c_void_p, c_ulong, c_ulong)
canDLL.ZCAN_InitCAN.argtypes = (c_void_p, c_ulong, c_void_p)
canDLL.ZCAN_InitCAN.restype = c_void_p
canDLL.ZCAN_StartCAN.argtypes = (c_void_p,)
canDLL.ZCAN_Transmit.argtypes = (c_void_p, c_void_p, c_ulong)
canDLL.ZCAN_TransmitFD.argtypes = (c_void_p, c_void_p, c_ulong)
canDLL.ZCAN_GetReceiveNum.argtypes = (c_void_p, c_ulong)
canDLL.ZCAN_Receive.argtypes = (c_void_p, c_void_p, c_ulong, c_long)
canDLL.ZCAN_ReceiveFD.argtypes = (c_void_p, c_void_p, c_ulong, c_long)
canDLL.ZCAN_ResetCAN.argtypes = (c_void_p,)
canDLL.ZCAN_CloseDevice.argtypes = (c_void_p,)
canDLL.ZCAN_ClearFilter.argtypes=(c_void_p,)
canDLL.ZCAN_AckFilter.argtypes=(c_void_p,)
canDLL.ZCAN_SetFilterMode.argtypes=(c_void_p,c_ulong)
canDLL.ZCAN_SetFilterStartID.argtypes=(c_void_p,c_ulong)
canDLL.ZCAN_SetFilterEndID.argtypes=(c_void_p,c_ulong)
#打开设备
m_dev = canDLL.ZCAN_OpenDevice(VCI_USBCAN2, 0, 0)
if m_dev == INVALID_DEVICE_HANDLE:
print("Open Device failed!")
exit(0)
print("Open Device OK, device handle:0x%x." %(m_dev))
#设置通道0 仲裁域波特率：1M, 数据域波特率：5M
ret = canDLL.ZCAN_SetAbitBaud(m_dev,0,1000000)
if ret != STATUS_OK:
print("Set CAN0 abit:1M failed!")
exit(0)
print("Set CAN0 abit:1M OK!")
ret = canDLL.ZCAN_SetDbitBaud(m_dev,0,4000000)
if ret != STATUS_OK:
print("Set CAN0 dbit:4M failed!")
exit(0)
print("Set CAN0 dbit:4M OK!")
# #设置通道1 仲裁域波特率：1M, 数据域波特率：5M
# ret = canDLL.ZCAN_SetAbitBaud(m_dev,1,1000000)
# if ret != STATUS_OK:
# print("Set CAN1 abit:1M failed!")
# exit(0)
# print("Set CAN1 abit:1M OK!");
# ret = canDLL.ZCAN_SetDbitBaud(m_dev,1,5000000)
# if ret != STATUS_OK:
# print("Set CAN1 dbit:5M failed!")
# exit(0)
# print("Set CAN1 dbit:5M OK!");
#设置通道0,1 FDMode : ISO
ret = canDLL.ZCAN_SetCANFDStandard(m_dev,0,0)
if ret != STATUS_OK:
print("Set CAN0 ISO mode failed!")
exit(0)
print("Set CAN0 ISO mode OK!")
# ret = canDLL.ZCAN_SetCANFDStandard(m_dev,1,0)
# if ret != STATUS_OK:
# print("Set CAN1 ISO mode failed!")
# exit(0)
# print("Set CAN1 ISO mode OK!")
#初始0通道
init_config = ZCAN_CHANNEL_INIT_CONFIG()
init_config.can_type = TYPE_CANFD
init_config.config.canfd.mode = 0
dev_ch1 = canDLL.ZCAN_InitCAN(m_dev, 0, byref(init_config))
if dev_ch1 == INVALID_CHANNEL_HANDLE:
print("Init CAN0 failed!")
exit(0)
print("Init CAN0 OK!")
#启动通道0
ret = canDLL.ZCAN_StartCAN(dev_ch1)
if ret != STATUS_OK:
print("Start CAN0 failed!")
exit(0)
print("Start CAN0 OK!")
# #初始1通道
# dev_ch2 = canDLL.ZCAN_InitCAN(m_dev, 1, byref(init_config))
# if dev_ch2 == INVALID_CHANNEL_HANDLE:
# print("Init CAN1 failed!")
# exit(0)
# print("Init CAN1 OK!")
###################################################################
# 在 ZCAN_InitCAN 之后， ZCAN_StartCAN之前配置
# 设置通道1 滤波:只接收 扩展帧,ID范围 5~6
###################################################################
# canDLL.ZCAN_ClearFilter(dev_ch2)
# canDLL.ZCAN_SetFilterMode(dev_ch2,1)
# canDLL.ZCAN_SetFilterStartID(dev_ch2,5)
# canDLL.ZCAN_SetFilterEndID(dev_ch2,6)
# canDLL.ZCAN_AckFilter(dev_ch2)
# #启动通道1
# ret = canDLL.ZCAN_StartCAN(dev_ch2)
# if ret != STATUS_OK:
# print("Start CAN1 failed!")
# exit(0)
# print("Start CAN1 OK!")
#################################
### CANFD frame send&&recv
#################################
# #通道1发送数据
# transmit_canfd_num = 10
# canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
# for i in range(transmit_canfd_num):
# canfd_msgs[i].transmit_type = 0 #0=正常发送，1=单次发送，2=自发自收，3=单次自发自收。
# canfd_msgs[i].frame.eff = 1 #extern frame
# canfd_msgs[i].frame.rtr = 0 #remote frame
# canfd_msgs[i].frame.brs = 1 #BRS
# canfd_msgs[i].frame.can_id = i
# canfd_msgs[i].frame.len = 16
# for j in range(canfd_msgs[i].frame.len):
# canfd_msgs[i].frame.data[j] = j
# ret = canDLL.ZCAN_TransmitFD(dev_ch1, canfd_msgs, transmit_canfd_num)
# print("\r\n CAN0 Tranmit CANFD Num: %d." % ret)
#通道2接收数据
#Receive Messages
# 初始化空数组
# data_array = []
start_time = time.time()
data_chunks = [] # 用于存储每帧ID为701的data_hex_max按八个字节一组的数组
# 创建 3D 图形对象
fig = plt.figure()
ax = fig.add_subplot(111)
# , projection='3d')
# 设置坐标轴范围
ax.set_xlim(-20, 20)
ax.set_ylim(0, 40)
# ax.set_zlim(-10, 10)
# 设置坐标轴标签
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
# ax.set_zlabel('Z Axis')
while True:
# 创建一个字典来存储ID为0x701的数据
# data_points = {}
ret = canDLL.ZCAN_GetReceiveNum(dev_ch1, TYPE_CANFD)
#print(ret)
while ret <= 0:#如果没有接收到数据，一直循环查询接收。
ret = canDLL.ZCAN_GetReceiveNum(dev_ch1, TYPE_CANFD)
if ret > 0:#接收到 ret 帧数据
rcv_canfd_msgs = (ZCAN_ReceiveFD_Data * ret)()
num = canDLL.ZCAN_ReceiveFD(dev_ch1, byref(rcv_canfd_msgs), ret, -1)
# print("CAN1 Received CANFD NUM: %d." % num)
for i in range(num):
# if rcv_canfd_msgs[i].frame.can_id == 0x701:
# data_hex_min = ' '.join(format(byte, '02X') for byte in rcv_canfd_msgs[i].frame.data[:rcv_canfd_msgs[i].frame.len])
# # 将data部分以十六进制方式打印出来
# data_points[i] = data_hex_min
# # 将数据以八个字节为一组放在一个点集里
if rcv_canfd_msgs[i].frame.can_id == 0x701:
data_hex_max = ' '.join(format(byte, '02X') for byte in rcv_canfd_msgs[i].frame.data[:rcv_canfd_msgs[i].frame.len])
print("[%d]: ts:%d, id:%x, len:%d, eff:%d, rtr:%d, esi:%d, brs: %d, data: %s" % (
i, rcv_canfd_msgs[i].timestamp, rcv_canfd_msgs[i].frame.can_id, rcv_canfd_msgs[i].frame.len,
rcv_canfd_msgs[i].frame.eff, rcv_canfd_msgs[i].frame.rtr,
rcv_canfd_msgs[i].frame.esi, rcv_canfd_msgs[i].frame.brs,
data_hex_max))
# 将data_hex_max按每八个字节一组进行分割
data_chunks.extend([data_hex_max[j:j+24] for j in range(0, len(data_hex_max), 24)])
# 检查是否已经过了0.1秒
elapsed_time = time.time() - start_time
if elapsed_time >= 0.1:
break
# 打印所有ID为701的data_hex_max数组
# 打印按每八个字节一组存储的数据
data_reall = []
# print("Data chunks for ID 701:")
# print(data_chunks[0])
for chunk in data_chunks:
# print(chunk)
hex_list = chunk.split()
# 将十六进制字符转换为对应的数值并存储在数组中
decimal_values = [int(hex_char, 16) for hex_char in hex_list]
# print("decimal_value:",decimal_values)
data_reall.append(PointCloundtoXY(decimal_values))
# print(data_reall[0])
print("data_reall*******************: ",data_reall)
print("data_len*********************: ",len(data_reall))
points = np.array(data_reall,dtype=np.dtype('f4'))
# 保留两位有效数字
points = np.around(points, decimals=2)
print("points1*********************: ",points)
# points = np.reshape(points,(int(points.shape[0]/4),4))
# 提取每一行的第三个元素（z坐标）
z_values = points[:,2]
# 检查z值是否在-1到+1范围内
mask = (z_values >= -0.2) & (z_values <= 0.2)
# mask = (z_values == 0)
# 只保留符合条件的点的所有坐标
pointsxyz = points[mask]
len_pointsxyz = len(pointsxyz)
# pointsxy=points[:,0:2]
print("len_pointsxyz: ",len_pointsxyz)
print("pointsxy*********************: ",pointsxyz)
# 二维平面图
for point in pointsxyz:
# print(data_xyz_vy)
# 绘制散点图
ax.scatter(point[0], point[1], c='r', marker='o', s=10)
# for point in pointsxyz:
# # print(data_xyz_vy)
# # 绘制散点图
# ax.scatter(point[0], point[1],point[2], c='r', marker='o', s=10)
plt.show()
# print(data_points)
# # 将字符串按空格分割成列表
# data_sets = [list(data_points.values())[i:i + 8] for i in range(0, len(data_points), 8)]
# # 从数组中提取字符串数据
# data_string = data_sets[0][0]
# # 将字符串按空格分割成列表
# data_list = data_string.split()
# # 将数据按照每八个字节为一行放入数组
# for i in range(0, len(data_list), 8):
# data_array.append(data_list[i:i+8])
# # 打印数组
# for row in data_array:
# print(row)
# time.sleep(0.1)
#################################
### CAN frame send&&recv
#################################
# #通道1发送数据
# transmit_can_num = 10
# can_msgs = (ZCAN_Transmit_Data * transmit_can_num)()
# for i in range(transmit_can_num):
# can_msgs[i].transmit_type = 0 #0=正常发送，1=单次发送，2=自发自收，3=单次自发自收。
# can_msgs[i].frame.eff = 1 #extern frame
# can_msgs[i].frame.rtr = 0 #remote frame
# #can_msgs[i].frame.brs = 1 #BRS
# can_msgs[i].frame.can_id = i
# can_msgs[i].frame.can_dlc = 8
# for j in range(can_msgs[i].frame.can_dlc):
# can_msgs[i].frame.data[j] = j
# ret = canDLL.ZCAN_Transmit(dev_ch1, can_msgs, transmit_can_num)
# print("\r\n CAN0 Tranmit CAN Num: %d." % ret)
# #通道2接收数据
# #Receive Messages
# ret = canDLL.ZCAN_GetReceiveNum(dev_ch2, TYPE_CAN)
# #print(ret)
# while ret <= 0:#如果没有接收到数据，一直循环查询接收。
# ret = canDLL.ZCAN_GetReceiveNum(dev_ch2, TYPE_CAN)
# if ret > 0:#接收到 ret 帧数据
# rcv_can_msgs = (ZCAN_Receive_Data * ret)()
# num = canDLL.ZCAN_Receive(dev_ch2, byref(rcv_can_msgs), ret, -1)
# print("CAN1 Received CAN NUM: %d." % num)
# for i in range(num):
# print("[%d]:ts:%d, id:%d, len:%d, eff:%d, rtr:%d, data:%s" %(
# i, rcv_can_msgs[i].timestamp, rcv_can_msgs[i].frame.can_id, rcv_can_msgs[i].frame.can_dlc,
# rcv_can_msgs[i].frame.eff, rcv_can_msgs[i].frame.rtr,
# ''.join(str(rcv_can_msgs[i].frame.data[j]) + ' ' for j in range(rcv_can_msgs[i].frame.can_dlc))))
#关闭
ret = canDLL.ZCAN_ResetCAN(dev_ch1)
if ret != STATUS_OK:
print("Close CAN0 failed!")
exit(0)
print("Close CAN0 OK!")
# ret = canDLL.ZCAN_ResetCAN(dev_ch2)
# if ret != STATUS_OK:
# print("Close CAN1 failed!")
# exit(0)
# print("Close CAN1 OK!")
ret = canDLL.ZCAN_CloseDevice(m_dev)
if ret != STATUS_OK:
print("Close Device failed!")
exit(0)
print("Close Device OK!")
代码解读3、碰见的一些问题：
把代码移植到函数进程中调用时，出现以下问题需要注意：
问题原因：在操作过程中频繁地开启与关闭CAN设备会导致其通信通道可能出现过于拥挤或状态波动的情况；每一次操作过程均涉及至 Least Squared 硬件接口的初始化与资源释放；大量操作可能导致资源争用以及系统状态不统一的情况出现
- 频繁开启关闭设备：在处理数据的过程中, 每次都需要启动CAN总线接口, 在完成数据读取后及时关闭该接口. 这种反复启停的操作可能会影响硬件运行稳定性.
- 异常处理机制：当启动通信端口或配置波特率出现故障时, 系统将直接终止运行. 这种情况在循环处理过程中会导致硬件资源过度使用问题
4、解决方法：
- 设备初始化与关闭分离：将设备的初始化与关闭操作独立于循环内部放置于循环外部执行层。这样可以在整个程序运行过程中仅执行一次初始化操作，并在程序退出时完成最后一次关闭。
- 错误处理增强：通过增强错误处理机制，在遇到不可恢复的情况（如无法开启设备或设置配置失败）时会短暂休眠后尝试重新启动相关组件。
- 数据接收流程优化：通过优化数据接收流程避免因设备状态异常导致的数据接收频繁中断。
5、类似解决方法的程序结构：
#打开设备，设波特率
def can_if_open(self):
judge_parameter = 0
m_dev = canDLL.ZCAN_OpenDevice(VCI_USBCAN2, 0, 0)
if m_dev == INVALID_DEVICE_HANDLE:
print("Open Device failed!")
# 设置通道0 仲裁域波特率：1M
ret_A = canDLL.ZCAN_SetAbitBaud(m_dev,0,1000000)
if ret_A != STATUS_OK:
print("Set CAN0 abit:1M failed!")
# 数据域波特率：4M
ret_D = canDLL.ZCAN_SetDbitBaud(m_dev,0,4000000)
if ret_D != STATUS_OK:
print("Set CAN0 dbit:4M failed!")
if m_dev == INVALID_DEVICE_HANDLE or ret_A != STATUS_OK or ret_D != STATUS_OK:
judge_parameter = 0
return judge_parameter,m_dev
else:
print("Open Device OK")
print("Set CAN0 abit:1M OK!")
print("Set CAN0 dbit:4M OK!")
judge_parameter = 1
return judge_parameter,m_dev
def initialize_can_interface(self,m_dev):
judge_parameter = 0
#初始0通道
dev_ch1 = canDLL.ZCAN_InitCAN(m_dev, 0, byref(self.init_config))
if dev_ch1 == INVALID_CHANNEL_HANDLE:
print("Init CAN0 failed!")
#启动通道0
ret = canDLL.ZCAN_StartCAN(dev_ch1)
if ret != STATUS_OK:
print("Start CAN0 failed!")
if dev_ch1 == INVALID_CHANNEL_HANDLE or ret != STATUS_OK:
judge_parameter = 0
return judge_parameter,dev_ch1
else:
print("Init CAN0 OK!")
print("Start CAN0 OK!")
judge_parameter = 1
return judge_parameter,dev_ch1
# 打开设备,设置波特率
if self.can_is_open == 0: # 没设置，没打开，则打开
self.can_is_open,m_dev = self.can_if_open()
self.log.info("Get 4D MMW_Radar_Data reopen dev !!!!!!!")
time.sleep(0.02)
continue
#初始0通道
if self.Is_initialize_can == 0: #没初始化，则初始化
self.Is_initialize_can,dev_ch1 = self.initialize_can_interface(m_dev)
self.log.info("Get 4D MMW_Radar_Data reintinal !!!!!!!")
time.sleep(0.02)
continue
代码解读6、结论：
在处理线程或频繁调用的函数时，在每次操作前不要直接开启或关闭can口端口以避免资源浪费问题。相反，在执行相关操作之前进行多次判断会更高效。如果已经打开过该端口，则无需再次开启；如果已经进行了初始化设置，则也无需重新进行初始化操作。这样可以在主循环内部直接调用该函数而不必每次都重新初始化和关闭设备。这种优化方法可以有效减少设备处于繁忙状态的时间，并提高程序的整体稳定性和运行效率
全部评论 (0)
还没有任何评论哟~