339 lines
9.6 KiB
C++
339 lines
9.6 KiB
C++
// can_comm.cpp - USBCANFD CAN/CANFD 收发封装实现
|
|
//
|
|
// 代码迁移自 vision-anti-pinch_bak/main.cpp 的 CAN 模块部分。
|
|
|
|
#include "can_comm.h"
|
|
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <pthread.h>
|
|
#include <unistd.h>
|
|
|
|
#include "zcan.h"
|
|
|
|
/* =====================================================================
|
|
* 类型/宏/全局变量
|
|
* ===================================================================== */
|
|
|
|
typedef uint32_t UINT;
|
|
typedef uint32_t U32;
|
|
#define msleep(ms) usleep((ms) * 1000)
|
|
|
|
#define USBCANFD 33
|
|
#define MAX_CHANNELS 2 // 最大通道数量
|
|
#define RX_WAIT_TIME 100
|
|
#define RX_BUFF_SIZE 1000
|
|
|
|
// 接收线程上下文
|
|
typedef struct
|
|
{
|
|
int dev_type; // 设备类型
|
|
int dev_idx; // 设备索引
|
|
int chn_idx; // 通道号
|
|
int total; // 接收总数
|
|
int stop; // 线程结束标志
|
|
} THREAD_CTX;
|
|
|
|
/* CAN 运行标志, 由信号处理函数(Ctrl+C)或外部置位 */
|
|
static volatile bool g_can_running = true;
|
|
|
|
static int g_receive_count = 0;
|
|
|
|
/* ===== 仅供 can_start/can_stop 共享的文件内静态变量 ===== */
|
|
static int s_DevType = USBCANFD;
|
|
static int s_DevIdx = 0;
|
|
static THREAD_CTX s_rx_ctx[MAX_CHANNELS];
|
|
static pthread_t s_rx_threads[MAX_CHANNELS];
|
|
static bool s_can_opened = false;
|
|
|
|
namespace CanComm {
|
|
|
|
bool is_open() { return s_can_opened; }
|
|
|
|
int receive_count() { return g_receive_count; }
|
|
|
|
/* ===== 报文解析回调 (由 main.cpp 通过 set_msg_callback 注册) ===== */
|
|
static MsgCallback s_msg_cb = nullptr;
|
|
|
|
void set_msg_callback(MsgCallback cb) { s_msg_cb = cb; }
|
|
|
|
} // namespace CanComm
|
|
|
|
/* =====================================================================
|
|
* 帧构建与收发
|
|
* ===================================================================== */
|
|
|
|
namespace CanComm {
|
|
|
|
// 构建 CAN 帧
|
|
static void construct_can_frame(ZCAN_20_MSG *can_msg, UINT id, int chn_idx, int pad)
|
|
{
|
|
memset(can_msg, 0, sizeof(ZCAN_20_MSG));
|
|
can_msg->hdr.inf.txm = 0; // 0-正常发送
|
|
can_msg->hdr.inf.fmt = 0; // 0-CAN
|
|
can_msg->hdr.inf.sdf = 0; // 0-数据帧, 1-远程帧
|
|
can_msg->hdr.inf.sef = 0; // 0-标准帧, 1-扩展帧
|
|
|
|
can_msg->hdr.id = id; // ID
|
|
can_msg->hdr.chn = chn_idx; // 通道
|
|
can_msg->hdr.len = 8; // 数据长度
|
|
|
|
if (pad > 0)
|
|
{
|
|
can_msg->hdr.pad = pad; // 发送后延迟 pad ms
|
|
can_msg->hdr.inf.qsend = 1; // 队列发送帧,仅判断首帧
|
|
can_msg->hdr.inf.qsend_100us = 0; // 队列发送单位,0-ms,1-100us
|
|
}
|
|
|
|
for (int i = 0; i < can_msg->hdr.len; i++)
|
|
can_msg->dat[i] = i;
|
|
}
|
|
|
|
// 构建 CANFD 帧
|
|
static void construct_canfd_frame(ZCAN_FD_MSG *canfd_msg, UINT id, int chn_idx, int pad)
|
|
{
|
|
memset(canfd_msg, 0, sizeof(ZCAN_FD_MSG));
|
|
canfd_msg->hdr.inf.txm = 0; // 0-正常发送
|
|
canfd_msg->hdr.inf.fmt = 1; // 0-CAN, 1-CANFD
|
|
canfd_msg->hdr.inf.sdf = 0; // 0-数据帧 CANFD只有数据帧!
|
|
canfd_msg->hdr.inf.sef = 1; // 0-标准帧, 1-扩展帧
|
|
canfd_msg->hdr.inf.brs = 1; // canfd 加速
|
|
|
|
canfd_msg->hdr.id = id; // ID
|
|
canfd_msg->hdr.chn = chn_idx; // 通道
|
|
canfd_msg->hdr.len = 64; // 数据长度
|
|
|
|
if (pad > 0)
|
|
{
|
|
canfd_msg->hdr.pad = 100; // 发送后延迟100ms
|
|
canfd_msg->hdr.inf.qsend = 1; // 队列发送帧,仅判断首帧
|
|
canfd_msg->hdr.inf.qsend_100us = 0; // 队列发送单位,0-ms,1-100us
|
|
}
|
|
|
|
for (int i = 0; i < canfd_msg->hdr.len; i++)
|
|
canfd_msg->dat[i] = i;
|
|
}
|
|
|
|
// 分发报文到 main.cpp 注册的解析回调
|
|
static inline void dispatch_msg(uint32_t ts, uint32_t msg_id, const uint8_t *data, uint32_t len)
|
|
{
|
|
if (s_msg_cb)
|
|
s_msg_cb(ts, msg_id, data, len);
|
|
}
|
|
|
|
// 接收线程
|
|
static void *rx_thread(void *data)
|
|
{
|
|
THREAD_CTX *ctx = (THREAD_CTX *)data;
|
|
int DevType = ctx->dev_type;
|
|
int DevIdx = ctx->dev_idx;
|
|
int chn_idx = ctx->chn_idx;
|
|
|
|
ZCAN_20_MSG can_data[RX_BUFF_SIZE];
|
|
ZCAN_FD_MSG canfd_data[RX_BUFF_SIZE];
|
|
|
|
while (!ctx->stop)
|
|
{
|
|
memset(can_data, 0, sizeof(can_data));
|
|
memset(canfd_data, 0, sizeof(canfd_data));
|
|
|
|
int rcount = VCI_Receive(DevType, DevIdx, chn_idx, can_data, RX_BUFF_SIZE, RX_WAIT_TIME); // CAN
|
|
for (int i = 0; i < rcount; ++i)
|
|
{
|
|
if (can_data[i].hdr.inf.sdf == 0)
|
|
{ // 数据帧
|
|
dispatch_msg(can_data->hdr.ts, can_data[i].hdr.id, can_data[i].dat, can_data[i].hdr.len);
|
|
}
|
|
}
|
|
ctx->total += rcount;
|
|
|
|
rcount = VCI_ReceiveFD(DevType, DevIdx, chn_idx, canfd_data, RX_BUFF_SIZE, RX_WAIT_TIME); // CANFD
|
|
for (int i = 0; i < rcount; ++i)
|
|
{
|
|
dispatch_msg(canfd_data->hdr.ts, canfd_data[i].hdr.id, canfd_data[i].dat, canfd_data[i].hdr.len);
|
|
}
|
|
ctx->total += rcount;
|
|
msleep(10);
|
|
}
|
|
g_receive_count = ctx->total;
|
|
printf("chn: %d receive %d\n", chn_idx, ctx->total);
|
|
pthread_exit(0);
|
|
}
|
|
|
|
// 合并接收线程
|
|
static void *rx_merge_thread(void *data)
|
|
{
|
|
THREAD_CTX *ctx = (THREAD_CTX *)data;
|
|
int DevType = ctx->dev_type;
|
|
int DevIdx = ctx->dev_idx;
|
|
|
|
ZCANDataObj obj_data[RX_BUFF_SIZE];
|
|
|
|
while (!ctx->stop)
|
|
{
|
|
memset(obj_data, 0, sizeof(obj_data));
|
|
int rcount = VCI_ReceiveData(DevType, DevIdx, 0, obj_data, RX_BUFF_SIZE, RX_WAIT_TIME); // 第三个参数固定为0就好
|
|
for (int i = 0; i < rcount; ++i)
|
|
{
|
|
if (obj_data[i].dataType == ZCAN_DT_ZCAN_CAN_DATA)
|
|
{ // CAN
|
|
ZCAN_20_MSG *can_data = &obj_data[i].data.zcanCANData;
|
|
if (can_data->hdr.inf.sdf == 0)
|
|
{ // 数据帧
|
|
dispatch_msg(can_data->hdr.ts, can_data->hdr.id, can_data->dat, can_data->hdr.len);
|
|
}
|
|
}
|
|
else if (obj_data[i].dataType == ZCAN_DT_ZCAN_CANFD_DATA)
|
|
{ // CANFD
|
|
ZCAN_FD_MSG *canfd_data = &obj_data[i].data.zcanCANFDData;
|
|
dispatch_msg(canfd_data->hdr.ts, canfd_data->hdr.id, canfd_data->dat, canfd_data->hdr.len);
|
|
}
|
|
}
|
|
ctx->total += rcount;
|
|
msleep(10);
|
|
}
|
|
g_receive_count = ctx->total;
|
|
printf("merge receive %d\n", ctx->total);
|
|
pthread_exit(0);
|
|
}
|
|
|
|
/*
|
|
* 初始化并启动 CAN 收发。
|
|
* 返回 true 成功; false 失败(主流程可继续仅取图)。
|
|
*/
|
|
bool can_start()
|
|
{
|
|
g_can_running = true;
|
|
int IsMerge = 1; // 是否合并接收
|
|
|
|
// 打开设备
|
|
if (!VCI_OpenDevice(s_DevType, s_DevIdx, 0))
|
|
{
|
|
printf("Open device fail\n");
|
|
return false;
|
|
}
|
|
s_can_opened = true;
|
|
printf("Open device success\n");
|
|
|
|
// 初始化,启动通道
|
|
for (int i = 0; i < MAX_CHANNELS; i++)
|
|
{
|
|
ZCAN_INIT init; // 波特率结构体,数据根据zcanpro的波特率计算器得出
|
|
init.clk = 60000000; // clock: 60M(V1.01) 80M(V1.03即以上)
|
|
init.mode = 0; // 0-正常
|
|
|
|
init.aset.tseg1 = 14; // 仲裁域 500kbps
|
|
init.aset.tseg2 = 3;
|
|
init.aset.sjw = 2;
|
|
init.aset.smp = 0;
|
|
init.aset.brp = 5;
|
|
|
|
init.dset.tseg1 = 10; // 数据域 2000kbps
|
|
init.dset.tseg2 = 2;
|
|
init.dset.sjw = 2;
|
|
init.dset.smp = 0;
|
|
init.dset.brp = 1;
|
|
|
|
if (!VCI_InitCAN(s_DevType, s_DevIdx, i, &init)) // 初始化通道
|
|
{
|
|
printf("InitCAN(%d) fail\n", i);
|
|
return false;
|
|
}
|
|
printf("InitCAN(%d) success\n", i);
|
|
|
|
U32 on = 1;
|
|
if (!VCI_SetReference(s_DevType, s_DevIdx, i, CMD_CAN_TRES, &on)) // 终端电阻
|
|
{
|
|
printf("CMD_CAN_TRES fail\n");
|
|
}
|
|
|
|
if (i == 0)
|
|
{
|
|
if (!VCI_SetReference(s_DevType, s_DevIdx, i, ZCAN_CMD_SET_CHNL_RECV_MERGE, &IsMerge)) // 合并接收
|
|
{
|
|
printf("ZCAN_CMD_SET_CHNL_RECV_MERGE fail\n");
|
|
}
|
|
}
|
|
|
|
if (!VCI_StartCAN(s_DevType, s_DevIdx, i)) // 启动通道
|
|
{
|
|
printf("StartCAN(%d) fail\n", i);
|
|
return false;
|
|
}
|
|
printf("StartCAN(%d) success\n", i);
|
|
|
|
s_rx_ctx[i].dev_type = s_DevType;
|
|
s_rx_ctx[i].dev_idx = s_DevIdx;
|
|
s_rx_ctx[i].chn_idx = i;
|
|
s_rx_ctx[i].total = 0;
|
|
s_rx_ctx[i].stop = 0;
|
|
|
|
if (IsMerge && i == 0)
|
|
{
|
|
pthread_create(&s_rx_threads[i], NULL, rx_merge_thread, &s_rx_ctx[i]); // 合并接收线程
|
|
}
|
|
else
|
|
{
|
|
pthread_create(&s_rx_threads[i], NULL, rx_thread, &s_rx_ctx[i]); // 创建接收线程
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
* 停止 CAN 接收并关闭设备。
|
|
*/
|
|
void can_stop()
|
|
{
|
|
if (!s_can_opened)
|
|
return;
|
|
|
|
g_can_running = false;
|
|
|
|
for (int i = 0; i < MAX_CHANNELS; i++)
|
|
{
|
|
s_rx_ctx[i].stop = 1;
|
|
pthread_join(s_rx_threads[i], NULL); // 等待接收线程退出
|
|
|
|
if (!VCI_ResetCAN(s_DevType, s_DevIdx, i)) // 复位通道
|
|
printf("ResetCAN(%d) fail\n", i);
|
|
else
|
|
printf("ResetCAN(%d) success!\n", i);
|
|
}
|
|
|
|
if (!VCI_CloseDevice(s_DevType, s_DevIdx))
|
|
printf("CloseDevice fail\n");
|
|
else
|
|
printf("CloseDevice success\n");
|
|
|
|
s_can_opened = false;
|
|
}
|
|
|
|
uint32_t can_send(int chn_idx, uint32_t id, const uint8_t data[8])
|
|
{
|
|
if (!s_can_opened)
|
|
return 0;
|
|
|
|
ZCAN_20_MSG msg;
|
|
construct_can_frame(&msg, id, chn_idx, 0);
|
|
for (int i = 0; i < 8; ++i)
|
|
msg.dat[i] = data[i];
|
|
|
|
return VCI_Transmit(s_DevType, s_DevIdx, chn_idx, &msg, 1);
|
|
}
|
|
|
|
uint32_t canfd_send(int chn_idx, uint32_t id, const uint8_t data[64])
|
|
{
|
|
if (!s_can_opened)
|
|
return 0;
|
|
|
|
ZCAN_FD_MSG msg;
|
|
construct_canfd_frame(&msg, id, chn_idx, 0);
|
|
for (int i = 0; i < 64; ++i)
|
|
msg.dat[i] = data[i];
|
|
|
|
return VCI_TransmitFD(s_DevType, s_DevIdx, chn_idx, &msg, 1);
|
|
}
|
|
|
|
} // namespace CanComm
|