1、硬件：
xavier-nx、创芯科技linux版本canfd盒子、纳雷科技SR-75 4D毫米波雷达（支持波特率4M的总线协议，can最多只能到1M，所以用canfd）
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设备的操作，这可能会导致CAN口变得繁忙或不稳定。每次打开和关闭设备都涉及到硬件接口的初始化和释放，频繁的操作可能会导致资源竞争或设备状态不一致。
- 频繁开关设备：在循环中，每次接收数据前都打开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)
continue6、结论：
在线程或者重复调用的函数中，不要直接循环开启关闭can口，这样会导致资源的复用和占据，应该将其进行多次的判断，如果打开过就不要再打开，如果初始化过，就不要再初始化了。这样可以在主循环中调用它，而不需要每次都重新初始化和关闭设备。这样可以减少设备繁忙的问题，并提高程序的稳定性和效率
扫一扫
3228
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
