Files
DCU_APP/GD32E508_DCU_APP/comm/ethercat_can_bridge.cpp
T

464 lines
20 KiB
C++
Raw Normal View History

/**
* @file ethercat_can_bridge.cpp
* @brief EtherCAT PDO ↔ CAN 转发桥实现(1.5.56)。
*/
#include "comm/ethercat_can_bridge.h"
#include "device/canfd.h"
#include "device/motor_state.h"
#include "RTE_Components.h"
#include CMSIS_device_header /* __DMB():seqlock 的编译器/总线屏障 */
#include <cstring>
volatile uint8_t EthercatCanBridge::s_lastSeq = 0U;
volatile bool EthercatCanBridge::s_seqPrimed = false;
volatile uint8_t EthercatCanBridge::s_lastTxResult = 0U;
volatile uint16_t EthercatCanBridge::s_txCount = 0U;
volatile uint16_t EthercatCanBridge::s_dropCount = 0U;
volatile uint8_t EthercatCanBridge::s_winBase = 0U;
volatile bool EthercatCanBridge::s_rxValid = false;
volatile uint32_t EthercatCanBridge::s_xrxSeq = 0U;
volatile uint8_t EthercatCanBridge::s_rxCh = 0U;
volatile uint8_t EthercatCanBridge::s_rxDlc = 0U;
volatile bool EthercatCanBridge::s_rxTruncated = false;
volatile uint32_t EthercatCanBridge::s_rxId = 0U;
volatile uint32_t EthercatCanBridge::s_rxCount = 0U;
uint8_t EthercatCanBridge::s_rxData[kEcatCanXRxDataMax] = {0};
EthercatCanBridge::RxRingItem
EthercatCanBridge::s_rxRing[kEcatRxRingSlots] = {};
volatile uint8_t EthercatCanBridge::s_rxRingHead = 0U;
volatile uint8_t EthercatCanBridge::s_rxRingTail = 0U;
volatile uint8_t EthercatCanBridge::s_rxRingLost = 0U;
namespace {
/** 小端读写:PDO 字节序按 EtherCAT 惯例固定小端,不依赖编译器结构体布局。 */
inline uint32_t rd32le(const uint8_t *p)
{
return (uint32_t)p[0] | ((uint32_t)p[1] << 8) |
((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}
inline void wr32le(uint8_t *p, uint32_t v)
{
p[0] = (uint8_t)(v); p[1] = (uint8_t)(v >> 8);
p[2] = (uint8_t)(v >> 16); p[3] = (uint8_t)(v >> 24);
}
inline void wr16le(uint8_t *p, uint16_t v)
{
p[0] = (uint8_t)(v); p[1] = (uint8_t)(v >> 8);
}
} // namespace
uint8_t EthercatCanBridge::sendBatch(const uint8_t *out)
{
uint8_t want = out[ECAT_TXB_OFF_COUNT];
if (want == 0U) {
return 0U; /* 未使用批量区:行为与 1.5.55 完全一致 */
}
if (want > kEcatTxBatchMax) {
want = kEcatTxBatchMax;
}
const bool stopOnError = (out[ECAT_TXB_OFF_FLAGS] & 0x01U) != 0U;
uint8_t sent = 0U;
uint16_t off = ECAT_TXB_OFF_ITEMS;
for (uint8_t i = 0U; i < want; ++i) {
/* 不信任主站给的 count/dlc:每步都验证头与数据都完整落在 PDO 内。
* 越界即停 —— 这是唯一的边界判据,count 只是上限提示。 */
if ((uint16_t)(off + ECAT_FRM_HDR_SIZE) > kEcatPdoOutBytes) {
break;
}
const uint8_t *f = &out[off];
uint8_t dlc = f[ECAT_FRM_OFF_DLC];
if (dlc > kEcatCanFdDataMax) {
dlc = kEcatCanFdDataMax;
}
if ((uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc) > kEcatPdoOutBytes) {
break;
}
const uint8_t ch = f[ECAT_FRM_OFF_CH];
CanFd::CanTxResult r;
if (!CanFd::isChannelEnabled(ch)) {
r = CanFd::CanTxResult::kErrInvalid;
} else {
/* 与单帧路径同样用 enqueueTx:批量下发必然连续打同一通道,
* transmitFrame 的 allow_abort 会让后帧顶掉前帧(历史上 7 帧
* 只发出 3 帧的根因),队列语义才不静默丢帧。 */
r = CanFd::enqueueTx(ch, rd32le(&f[ECAT_FRM_OFF_ID]),
&f[ECAT_FRM_HDR_SIZE], dlc);
}
if (r == CanFd::CanTxResult::kOk) {
++s_txCount;
++sent;
} else {
s_lastTxResult = (uint8_t)r;
if (stopOnError) {
break;
}
}
off = (uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc);
}
return sent;
}
void EthercatCanBridge::fillRxBatch(uint8_t *in)
{
uint8_t count = 0U;
uint16_t off = ECAT_RXB_OFF_ITEMS;
uint8_t tail = s_rxRingTail;
const uint8_t head = s_rxRingHead; /* 快照一次:生产者可能边填边推进 */
while (tail != head && count < kEcatRxBatchMax) {
/* 整条先拷到局部副本再用:生产者(主循环 onCanFrame)与本函数(PDI_Isr)
* 共用这块槽,直接按引用逐字段取会让 id/dlc/data 分属不同帧。 */
const RxRingItem it = s_rxRing[tail];
uint8_t dlc = it.dlc;
if (dlc > kEcatCanFdDataMax) {
dlc = kEcatCanFdDataMax;
}
/* 1.6.1: PDO 内数据条目 32B 上限——DLC 字节填**真值**,超出部分截断并打
* flags bit1;环形队列里仍是全量 64B,截断只发生在 PDO 快照这一步。
* 主站按 "8 + min(dlc, kEcatRxBatchDataMax)" 推进游标。 */
uint8_t dlcWire = dlc;
if (dlcWire > kEcatRxBatchDataMax) {
dlcWire = kEcatRxBatchDataMax;
}
/* 装不下就停,**且不推进 tail** —— 该帧留到下个周期,不丢。
* 边界是段 5 诊断区起点(ECAT_DIAG_OFF_LOAD=484),不是 PDO 全长(496):
* 1.6.10 修正。旧判据允许条目尾部最多越进诊断区 12B,把负载率/撕裂计数/
* 序号盖成帧数据 —— 当前 kEcatRxBatchMax=3 时最坏占用恰好 484 触不到,
* 属潜伏 bug,但容量一旦按瘦身布局放宽(32B 步长)就会立刻咬上来。 */
if ((uint16_t)(off + ECAT_FRM_HDR_SIZE + dlcWire) > ECAT_DIAG_OFF_LOAD) {
break;
}
uint8_t *p = &in[off];
p[ECAT_FRM_OFF_CH] = it.ch;
p[ECAT_FRM_OFF_DLC] = dlc;
p[ECAT_FRM_OFF_FLAGS] = (uint8_t)(0x01U | (dlc > kEcatRxBatchDataMax ? 0x02U : 0x00U));
wr32le(&p[ECAT_FRM_OFF_ID], it.id);
memcpy(&p[ECAT_FRM_HDR_SIZE], it.data, dlcWire);
off = (uint16_t)(off + ECAT_FRM_HDR_SIZE + dlcWire);
tail = (uint8_t)((tail + 1U) % kEcatRxRingSlots);
++count;
/* **每条读完立刻发布 tail**(1.6.9)。原来攒到循环末才发布:生产者判满
* 用的正是这个滞后的 s_rxRingTail,于是它以为队列里还有本拍已被读走的
* 那几个空位没腾出来 —— 反过来说,它会把新帧写进消费者**刚读过但未发布**
* 的槽。队列一旦饱和(实测 pending 恒 8/12、lost 几乎每拍都有、count 恒
* 取满 3),这个窗口每拍都在,于是拷进 PDO 的 id/dlc/data 来自不同帧,
* 表现为"CONFIG 的 ID 配 STATUS 的内容"这类错标(实测 1.77%)。
* 逐条发布后生产者的判满始终基于真实进度,覆写窗口消失。 */
__DMB(); /* 本条读取先于 tail 发布落地 */
s_rxRingTail = tail;
}
const uint8_t pending = (uint8_t)((head + kEcatRxRingSlots - tail) % kEcatRxRingSlots);
in[ECAT_RXB_OFF_COUNT] = count;
in[ECAT_RXB_OFF_LOST] = s_rxRingLost;
in[ECAT_RXB_OFF_PENDING] = pending;
s_rxRingLost = 0U; /* 回填后清零:lost 是"自上次回填以来"的增量 */
}
bool EthercatCanBridge::peekOutput(const uint8_t *head)
{
if (head == nullptr) {
return false;
}
const uint8_t seq = head[ECAT_CAN_TX_OFF_SEQ];
/* 窗口下标每周期都必须刷新(主站靠滚动 WIN 扫 27 槽),与 seq 沿无关。
* 越界值钳到 0,避免读到槽数组外(与 processOutput 同款钳制)。 */
const uint8_t win = head[ECAT_CAN_TX_OFF_WIN];
s_winBase = (win < kMotorSlotCount) ? win : 0U;
/* 上电/重连后首帧只记基准(与 processOutput 的 priming 分支语义一致) */
if (!s_seqPrimed) {
s_seqPrimed = true;
s_lastSeq = seq;
return false;
}
/* seq 沿 → 全量读;FLAGS bit0(清统计请求)允许不带 seq 沿(同 processOutput)。
* s_lastSeq 不在这里推进:全量路径上 processOutput 的沿处理只做一次。 */
if (seq != s_lastSeq) {
return true;
}
return (head[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U;
}
void EthercatCanBridge::processOutput(const uint8_t *out)
{
if (out == nullptr) {
return;
}
const uint8_t seq = out[ECAT_CAN_TX_OFF_SEQ];
/* WIN 不在这里取:1.6.0 起 out 只在 seq 沿那拍才是新鲜的,无沿的周期里它是
* 上一次全量读的残值。在这里回写 s_winBase,等于用开机首拍的 0 每拍盖掉
* peekOutput() 刚取到的新值 —— 窗口被钉死在基址 0,槽位 7..26(node3 及
* 以上)永不上报,直到主站真发一帧触发一次全量读才复位。
* 每拍刷新由 peekOutput()(SSC PDO_OutputMapping 的 12B 探针)独占负责。 */
/* 上电/重连后第一次看到输出数据时只记录基准,不发送:否则主站残留的
* 非零 seq 会被当成一次新请求,在进 OP 的瞬间凭空发出一帧。 */
if (!s_seqPrimed) {
s_seqPrimed = true;
s_lastSeq = seq;
return;
}
/* flags bit0:清零统计(一次性,不依赖 seq 沿,便于主站随时归零) */
if ((out[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U) {
s_txCount = 0U;
s_dropCount = 0U;
s_rxCount = 0U;
}
if (seq == s_lastSeq) {
return; /* 没有新的发送请求 */
}
s_lastSeq = seq;
const uint8_t ch = out[ECAT_CAN_TX_OFF_CH];
uint8_t dlc = out[ECAT_CAN_TX_OFF_DLC];
if (dlc > kEcatCanTxDataMax) {
dlc = kEcatCanTxDataMax;
}
/* 单帧区先发(保持 1.5.55 语义),再发批量区。
* dlc==0 表示本次只用批量区,不发单帧 —— 否则会凭空发出一帧 0 长度报文。 */
if (dlc > 0U) {
/* isChannelEnabled 内含越界检查,是"通道号合法且可用"的唯一判据 */
if (!CanFd::isChannelEnabled(ch)) {
s_lastTxResult = (uint8_t)CanFd::CanTxResult::kErrInvalid;
} else {
/* 用 enqueueTx 而非 transmitFrame:PDO 是周期性场景,可能连续多帧打到
* 同一通道。transmitFrame 默认 allow_abort=true 会在邮箱满时顶掉已装载
* 的旧帧(历史上 7 帧突发只发出 3 帧的根因);enqueueTx 队列空时直通、
* 满时排队,不静默丢帧。 */
const CanFd::CanTxResult r =
CanFd::enqueueTx(ch, rd32le(&out[ECAT_CAN_TX_OFF_ID]),
&out[ECAT_CAN_TX_OFF_DATA], dlc);
s_lastTxResult = (uint8_t)r;
if (r == CanFd::CanTxResult::kOk) {
++s_txCount;
}
}
}
/* 批量区:同一个 seq 沿里一次性发 N 帧,供多轴同步下发。 */
(void)sendBatch(out);
}
void EthercatCanBridge::buildInput(uint8_t *in)
{
if (in == nullptr) {
return;
}
/* 必须按新的全长清零。旧代码写死 memset(in,0,32),扩容后会把 32 字节以后
* 的槽区留成上一周期的残留数据,主站读到的是"看起来合法但已过期"的电机状态。 */
memset(in, 0, kEcatPdoInBytes);
/* ── 段 1(0..31):桥状态 + 短快照,布局与 1.5.53 逐字节兼容 ── */
in[ECAT_CAN_RX_OFF_ACK] = s_lastSeq;
in[ECAT_CAN_RX_OFF_TXRES] = s_lastTxResult;
wr16le(&in[ECAT_CAN_RX_OFF_TXCNT], s_txCount);
wr16le(&in[ECAT_CAN_RX_OFF_DROP], s_dropCount);
wr32le(&in[ECAT_CAN_RX_OFF_RXCNT], s_rxCount);
/* seqlock 判稳(见 onCanFrame):奇数=生产者正在写这组快照。单核下 ISR
* 执行期间生产者冻结,读一次即可定性;本周期跳过段 1/段 3(开头已清零,
* 语义=无有效快照),下周期自愈。批量区(段 4)走环形队列,不受影响。 */
const bool xrxStable = ((s_xrxSeq & 1U) == 0U);
if (xrxStable && s_rxValid) {
uint8_t flags = 0x01U;
if (s_rxTruncated) {
flags |= 0x02U;
}
in[ECAT_CAN_RX_OFF_CH] = s_rxCh;
in[ECAT_CAN_RX_OFF_DLC] = s_rxDlc;
in[ECAT_CAN_RX_OFF_FLAGS] = flags;
wr32le(&in[ECAT_CAN_RX_OFF_ID], s_rxId);
/* 段 1 只放前 8 字节(兼容),完整 payload 见段 3 */
memcpy(&in[ECAT_CAN_RX_OFF_DATA], s_rxData, kEcatCanRxDataMax);
}
const uint8_t ch = s_rxValid ? s_rxCh : 0U;
const uint32_t backlog = CanFd::isChannelEnabled(ch) ? CanFd::txQueueBacklog(ch) : 0U;
in[ECAT_CAN_RX_OFF_BACKLOG] = (backlog > 255U) ? 255U : (uint8_t)backlog;
/* ── 段 2(32..319):电机状态窗口 ──
* 直接读 MotorState 聚合表,不再依赖"最近一帧 CAN"。窗口起点由主站通过
* 输出 PDO 的 WIN 字节指定,主站每周期 +5 即可 6 周期扫完 27 个槽。
*
* 上下文安全(1.6.5 修正):状态表由主循环的 taskCanRxParse 写,而本函数跑在
* PDI_Isr(SM 同步模式)/Sync0_Isr(DC 模式),会抢占写侧那次
* memcpy(slot.data, 48B)。旧注释称"最坏是 seq 与 data 不同步一拍,主站按 seq
* 判新旧即可自愈"是错的:写侧先 seq 后 data,抢占留下的恰好是"新 seq +
* 半新半旧 data",而主站只拿 seq 判新鲜度,分辨不出同一 seq 下 payload 换过
* 内容,混合值照原样进界面(边界落在 B16/B20/B24 这类 float 符号/指数字节上
* 就是离谱值)。改用与段 1/段 3 同一套 seqlock(MotorSlot.wr_seq):奇数即
* 跳过本槽,与本片既有的"本拍不报、下拍自愈"约定一致。 */
const MotorStateTable &mt = MotorState::table();
const uint8_t base = (s_winBase < kMotorSlotCount) ? s_winBase : 0U;
in[ECAT_MOTW_OFF_ACTIVE] = mt.active_count;
in[ECAT_MOTW_OFF_BOOT] = mt.boot_active;
in[ECAT_MOTW_OFF_WIN_BASE] = base;
wr32le(&in[ECAT_MOTW_OFF_GLOBAL_SEQ], mt.global_seq);
uint8_t count = 0U;
for (uint8_t i = 0U; i < kEcatMotSlotsPerPdo; ++i) {
const uint8_t idx = (uint8_t)(base + i);
if (idx >= kMotorSlotCount) {
break; /* 窗口尾部越界即停,不回绕:主站据 win_count 判断 */
}
const MotorSlot &s = mt.slots[idx];
uint8_t *p = &in[ECAT_MOTW_OFF_SLOTS + (uint16_t)i * kEcatMotSlotSize];
/* 槽按 i 定位,跳过的那格照样占位,所以 win_count 报的是跨度而不是实际
* 填充数:主站 decode_input 对 node_id==0 的块直接 continue。 */
count = static_cast<uint8_t>(i + 1U);
if ((s.wr_seq & 1U) != 0U) {
continue; /* 生产者正在写这 48B:本拍留零,下拍自愈 */
}
__DMB(); /* 判稳先于取值:不许把下面的读重排到奇偶检查之前 */
p[ECAT_MOT_OFF_NODE] = s.node_id;
p[ECAT_MOT_OFF_CH] = s.channel;
p[ECAT_MOT_OFF_ONLINE] = s.online;
p[ECAT_MOT_OFF_DLC] = s.dlc;
wr32le(&p[ECAT_MOT_OFF_SEQ], s.seq);
memcpy(&p[ECAT_MOT_OFF_STATUS], s.data, kMotorStatusDlc);
}
in[ECAT_MOTW_OFF_WIN_COUNT] = count;
/* ── 段 3(320..359):最近一帧完整快照 ──
* 版本帧(0x00FF)/CONFIG 帧(0x0101) 都是 DLC 24,段 1 的 8 字节装不下。
* 与段 1 共用 xrxStable 门控:两段必须来自同一帧,否则 ecat.html 的短快照
* 和电机页的长快照会各自解出半新半旧的 ID/数据。 */
if (xrxStable && s_rxValid) {
in[ECAT_XRX_OFF_CH] = s_rxCh;
in[ECAT_XRX_OFF_DLC] = s_rxDlc;
in[ECAT_XRX_OFF_FLAGS] = 0x01U;
wr32le(&in[ECAT_XRX_OFF_ID], s_rxId);
memcpy(&in[ECAT_XRX_OFF_DATA], s_rxData, kEcatCanXRxDataMax);
}
/* ── 段 4(360..639):CANFD 批量接收区 ──
* 段 1/段 3 只带"最近一帧",高速多帧场景下周期间到达的帧会互相覆盖
* (s_dropCount 就是在数这个)。批量区从环形队列里一次回收多帧,
* 让主站在一个周期内拿到完整的帧序列而不是采样点。 */
fillRxBatch(in);
}
void EthercatCanBridge::onCanFrame(uint8_t channel, uint32_t id,
const uint8_t *data, uint8_t length)
{
if (data == nullptr) {
return;
}
/* 上一帧还没被 PDO 周期取走就来了新帧:记一次丢弃(覆盖旧帧,
* 保证主站看到的永远是最新状态,而不是卡在一帧陈旧数据上)。 */
if (s_rxValid) {
++s_dropCount;
}
/* 缓存区已扩到 32B(段 3 用),truncated 仍按段 1 的 8B 口径上报,
* 这样 ecat.html 的 CAN 调试页语义不变。 */
uint8_t copyLen = length;
if (copyLen > kEcatCanXRxDataMax) {
copyLen = kEcatCanXRxDataMax;
}
/* ── 快照 seqlock(1.5.58)──
* s_rxData/s_rxCh/s_rxId/s_rxDlc/s_rxTruncated/s_rxValid 是一组,必须整体
* 被 buildInput 读到。DC 模式下 buildInput 跑在 Sync0_Isr(preempt=0),会
* 抢占本函数(主循环 taskCanRxParse)。原注释声称"valid 最后置位保证一致"
* 是错的:valid 一旦置位就永不清零,ISR 落在"新 payload 已写完、id 还没
* 更新"的窗口里,会把新帧数据配旧帧 ID 发出去——STATUS 字节被标成 CONFIG
* (0x000N0101),前端按 CONFIG 布局解码出 -3e26 这类离谱目标值。
* 协议:seq 奇数=正在写。消费者在单核 ISR 里生产者必然冻结,判一次奇偶
* 即可,奇数则本周期跳过(段 1/段 3 已清零=无效),下周期自愈。
* 不关中断:critical.h 的 BASEPRI 方案放行 preempt=0,挡不住 Sync0;
* 而 seqlock 零抖动,与本文件既有无锁约定一致。
* __DMB 防编译器把非 volatile 的 memcpy 挪出两次 seq 自增之间。 */
++s_xrxSeq; /* → 奇数:快照进入"正在写" */
__DMB();
memcpy(s_rxData, data, copyLen);
if (copyLen < kEcatCanXRxDataMax) {
memset(&s_rxData[copyLen], 0, (size_t)(kEcatCanXRxDataMax - copyLen));
}
s_rxCh = channel;
s_rxId = id;
s_rxDlc = length; /* 报告原始长度,便于主站发现被截断 */
s_rxTruncated = (length > kEcatCanRxDataMax);
++s_rxCount;
s_rxValid = true;
__DMB();
++s_xrxSeq; /* → 偶数:快照整体一致 */
/* ── 推入批量接收环形队列(1.5.56)──
* 与上面的"最近一帧快照"并行维护:快照会被后帧覆盖(这是它的设计语义),
* 队列则保序累积,供 fillRxBatch 一次回收多帧。
* 队列满时丢**新**帧并计数,而不是覆盖旧帧 —— 批量区的价值在于保序完整,
* 覆盖会在序列中间留下空洞,比明确丢尾更难排查。
*
* **STATUS 帧不入队**(1.6.9):批量区存在的意义是捞那些段 2 装不下的帧
* (CONFIG / 版本 / 节点应答),而完整 STATUS 本来就由段 2 电机状态窗口按槽
* 上报;主站 transport-ecat.js 的 drainRx() 拿到 STATUS 也是直接 continue
* 丢掉。让它占队列是纯重复,而且正是它把队列压爆的:实测 CAN 侧约 4000 帧/s,
* 批量区每拍最多 3 条(=3000 帧/s),净积压使 pending 恒 8/12、lost 几乎每拍
* 都有 —— 队列长期饱和,才逼出了生产者覆写消费者槽位的竞态(错标率 1.77%,
* 表现为"CONFIG 的 ID 配 STATUS 的内容")。滤掉 STATUS 后入队流量降到 CONFIG
* 量级,队列不再饱和,竞态窗口随之消失。 */
if ((id & 0xFFFFU) == 0x0100U) {
return;
}
const uint8_t next = (uint8_t)((s_rxRingHead + 1U) % kEcatRxRingSlots);
if (next == s_rxRingTail) {
if (s_rxRingLost < 0xFFU) {
++s_rxRingLost;
}
} else {
RxRingItem &it = s_rxRing[s_rxRingHead];
uint8_t n = length;
if (n > kEcatCanFdDataMax) {
n = kEcatCanFdDataMax;
}
it.id = id;
it.ch = channel;
it.dlc = n;
memcpy(it.data, data, n);
__DMB(); /* 数据写入先于 head 发布落地(volatile 不约束非 volatile 的 memcpy) */
s_rxRingHead = next; /* 最后推进 head:消费者读到时数据必已写完 */
}
}
extern "C" {
void EcatCan_ProcessOutput(const uint8_t *out)
{
EthercatCanBridge::processOutput(out);
}
void EcatCan_BuildInput(uint8_t *in)
{
EthercatCanBridge::buildInput(in);
}
} /* extern "C" */