zmv/vision-anti-pinch
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Integrate CAN-aware CMS risk pipeline and relocatable Orin runtime

Browse Source
This commit is contained in:
zmv
2026-09-23 18:54:11 +08:00
parent 26c5bfb88d
commit 9aadf9cbda
119 changed files with 13940 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
can_comm.cpp
+338
View File
@@ -0,0 +1,338 @@
// 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