cbb
d8b3c9767c
进料口从「只有 ESP32 SPI 一条」扩成两条，中继引擎不变：主站经 CoE 0x2511 数据窗 把电机固件明文灌进 GD32 的 48KB SRAM 暂存区，再由 comm/ecat_motor_ota 泵进原有 4KB 环缓；AES 握手、CAN 单发停等、重发、0AAC 运行确认全部沿用 MotorOtaRelay。 纯网线（ESP32 WiFi 关断、/motor/ota 打不通）场合因此也能刷电机。 偏移/幂等判据仍只有一份：feedData 拆出 feedAt，两个进料口共用。 同时把审计报告 VULN-12/13/14 落地（此前只存在于文档，一行未实现）：start() 第一条 语句在 PRIMASK 内原子置位 s_channel_lock_mask，EtherCAT 桥接 sendBatch/processOutput 与 SPI 0xC0/0xC7 对锁定通道回新码 0xE7=「被电机 OTA 独占」，不冒充「邮箱满」0xE4。 反向互斥一并补上：电机 OTA 活跃时 requestEnterBoot() 直接拒绝——SPI 0xB0、CoE 0x2502、 CAN 0x320 三条交接入口共用这一个判据，避免复位把刷写腰斩在 erase 之后。 完整性口径按链路分开，不互相冒充：块级 CRC32 + 整镜像 CRC32 都在从站核，且核过才 让电机进 BOOT——失败时电机仍跑旧 APP、擦除没开始，不存在「先擦再验」的变砖路径。 CRC 为 CRC-32/B（poly 0x04C11DB7，初值 0xFFFFFFFF，无反射无末尾异或），与 v2_crc32 同算法（用公开向量 123456789=0x0376E6E7 对过）。 块长取 1024 而不是 BOOT 用的 4096：SSC 的分段下载缓冲是全工程唯一的 malloc 用户， 而 APP 的 __HEAP_SIZE 只有 3KB，4096 会被从站当场回 abort 0x05040005 Out of memory （BOOT 的堆是 16KB 才撑得住 4096——块长能抄，堆抄不了）。这是跨仓常量，须与 tools/ecat 的 MFO_CHUNK_OBJ 同步。 APP 1.7.4（RAM ZI 97156）。已实测：0x2510 参数写与一块 1016B 数据写均被从站接受 （staged=1016、phase bit4）；端到端电机刷写待复测（native 块长修复后未跑完整一轮）。
502 lines
17 KiB
C++
502 lines
17 KiB
C++
/**
|
||
* @file ethercat_can_bridge.cpp
|
||
* @brief EtherCAT PDO ↔ CAN 转发桥实现。
|
||
*/
|
||
#include "comm/ethercat_can_bridge.h"
|
||
#include "device/canfd.h"
|
||
#include "device/motor_state.h"
|
||
#include "sys/motor_ota_relay.h"
|
||
#include "sys/systick.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 bool EthercatCanBridge::s_coldStartPending = false;
|
||
volatile uint8_t EthercatCanBridge::s_lastTxResult = 0U;
|
||
volatile uint16_t EthercatCanBridge::s_txCount = 0U;
|
||
volatile uint16_t EthercatCanBridge::s_dropCount = 0U;
|
||
volatile uint32_t EthercatCanBridge::s_lastTxId = 0U;
|
||
volatile uint16_t EthercatCanBridge::s_rx00ffCount = 0U;
|
||
volatile uint8_t EthercatCanBridge::s_outSeenSeq = 0U;
|
||
|
||
volatile bool EthercatCanBridge::s_rxValid = false;
|
||
volatile bool EthercatCanBridge::s_rxPublished = 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};
|
||
|
||
volatile bool EthercatCanBridge::s_verValid = false;
|
||
volatile uint32_t EthercatCanBridge::s_verHoldUntilMs = 0U;
|
||
uint8_t EthercatCanBridge::s_verCh = 0U;
|
||
uint8_t EthercatCanBridge::s_verDlc = 0U;
|
||
uint32_t EthercatCanBridge::s_verId = 0U;
|
||
uint8_t EthercatCanBridge::s_verData[kEcatCanXRxDataMax] = {0};
|
||
|
||
namespace {
|
||
/** 版本应答在 PDO 段3 的保持时长（ms）。主站 1ms 周期下足够被采到。 */
|
||
constexpr uint32_t kVerHoldMs = 300U;
|
||
|
||
inline uint16_t canSubId(uint32_t id) { return static_cast<uint16_t>(id & 0xFFFFU); }
|
||
} // namespace
|
||
|
||
uint8_t EthercatCanBridge::s_motShadow[kEcatMotRegionBytes] = {0};
|
||
uint32_t EthercatCanBridge::s_motPubSeq[kMotorMaxNodes + 1U] = {0};
|
||
uint8_t EthercatCanBridge::s_motPubOnline[kMotorMaxNodes + 1U] = {0};
|
||
uint16_t EthercatCanBridge::s_motPubMask = 0U;
|
||
uint8_t EthercatCanBridge::s_motPubCount = 0U;
|
||
bool EthercatCanBridge::s_motShadowValid = false;
|
||
|
||
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);
|
||
}
|
||
|
||
/** 段 2 头 + count 槽在影子区中的总长度。 */
|
||
inline uint16_t motRegionLen(uint8_t count)
|
||
{
|
||
return (uint16_t)(kEcatMotHdrSize + (uint16_t)count * kEcatMotSlotSize);
|
||
}
|
||
} // namespace
|
||
|
||
uint8_t EthercatCanBridge::sendBatch(const uint8_t *out)
|
||
{
|
||
uint8_t want = out[ECAT_TXB_OFF_COUNT];
|
||
if (want == 0U) {
|
||
return 0U;
|
||
}
|
||
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) {
|
||
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 if (MotorOtaRelay::isChannelLocked(ch)) {
|
||
/* 该通道正被电机 OTA 独占：PDO 运动帧一律不投递。
|
||
* OTA 用单发模式(ARD=1)，一旦挤进同一个邮箱就会 abort 掉在途的
|
||
* 0x101 固件包，或让电机 BOOT 收到非预期长度直接回 0x05/0x06。
|
||
* 计数与 lastTxResult 走 0xE7，主站侧能看到「被 OTA 挡了」而不是「邮箱满」。 */
|
||
r = CanFd::CanTxResult::kErrOtaLocked;
|
||
} else {
|
||
r = CanFd::enqueueTx(ch, rd32le(&f[ECAT_FRM_OFF_ID]),
|
||
&f[ECAT_FRM_HDR_SIZE], dlc);
|
||
}
|
||
|
||
if (r == CanFd::CanTxResult::kOk) {
|
||
++s_txCount;
|
||
++sent;
|
||
s_lastTxId = rd32le(&f[ECAT_FRM_OFF_ID]);
|
||
} else {
|
||
s_lastTxResult = (uint8_t)r;
|
||
if (stopOnError) {
|
||
break;
|
||
}
|
||
}
|
||
off = (uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc);
|
||
}
|
||
return sent;
|
||
}
|
||
|
||
/**
|
||
* 段 2：电机整轮密排。
|
||
*
|
||
* 1. 扫 MotorState，每个节点号 1..9 在 3 个通道里取 seq 最新的一条；
|
||
* 2. 若节点集合与各节点 seq 均与上一拍发布相同 → 整段复用影子区（跳过重读）；
|
||
* 3. 否则按 node_id 升序密排写入，空节点不占位；seqlock 奇数（写入中）的槽
|
||
* 本拍省略，下拍自愈 —— 不发半新半旧的 48B。
|
||
*/
|
||
uint8_t EthercatCanBridge::fillMotorRound(uint8_t *in)
|
||
{
|
||
const MotorStateTable &mt = MotorState::table();
|
||
|
||
/* ── 1. 每节点选一条最新槽 ── */
|
||
const MotorSlot *picks[kMotorMaxNodes];
|
||
uint8_t nodes[kMotorMaxNodes];
|
||
uint8_t n = 0U;
|
||
|
||
for (uint8_t node = 1U; node <= kMotorMaxNodes; ++node) {
|
||
const MotorSlot *best = nullptr;
|
||
for (uint8_t ch = 0U; ch < kMotorChannels; ++ch) {
|
||
const MotorSlot &s =
|
||
mt.slots[(uint8_t)((node - 1U) * kMotorChannels + ch)];
|
||
if (s.node_id == 0U) {
|
||
continue;
|
||
}
|
||
if (best == nullptr || s.seq >= best->seq) {
|
||
best = &s;
|
||
}
|
||
}
|
||
if (best != nullptr) {
|
||
picks[n] = best;
|
||
nodes[n] = node;
|
||
++n;
|
||
}
|
||
}
|
||
|
||
/* ── 2. 判「上一轮已填充且未变」──
|
||
* 节点集合一致 + 每节点 seq/online 一致 + 头字段（active/boot）一致 + 写侧
|
||
* seqlock 偶数 → 复用影子。online 必须比：checkOffline 超时置 0 时不 bump
|
||
* seq，只比 seq 会把「已离线」永远报成在线。 */
|
||
bool unchanged = s_motShadowValid && (n == s_motPubCount)
|
||
&& mt.active_count == s_motShadow[ECAT_MOTW_OFF_ACTIVE - ECAT_MOTW_OFF_COUNT]
|
||
&& mt.boot_active == s_motShadow[ECAT_MOTW_OFF_BOOT - ECAT_MOTW_OFF_COUNT]
|
||
&& s_motShadow[ECAT_MOTW_OFF_MAGIC - ECAT_MOTW_OFF_COUNT]
|
||
== kEcatMotLayoutMagic;
|
||
if (unchanged) {
|
||
for (uint8_t i = 0U; i < n; ++i) {
|
||
const uint8_t node = nodes[i];
|
||
const MotorSlot &s = *picks[i];
|
||
if ((s.wr_seq & 1U) != 0U
|
||
|| s.seq != s_motPubSeq[node]
|
||
|| s.online != s_motPubOnline[node]
|
||
|| (s_motPubMask & (uint16_t)(1U << node)) == 0U) {
|
||
unchanged = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
const uint16_t region = motRegionLen(unchanged ? s_motPubCount : n);
|
||
|
||
/* 集合+seq+online 未变：整段复用影子，避免 8kHz 下每拍重建密排区。
|
||
* 头字段 ACTIVE/BOOT/MAGIC/GSEQ 在影子起点 COUNT 之后，一并复用。 */
|
||
if (unchanged && s_motShadowValid && s_motPubCount > 0U) {
|
||
memcpy(&in[ECAT_MOTW_OFF_COUNT], s_motShadow, region);
|
||
return s_motPubCount;
|
||
}
|
||
|
||
/* ── 3. 重建密排区 ── */
|
||
in[ECAT_MOTW_OFF_ACTIVE] = mt.active_count;
|
||
in[ECAT_MOTW_OFF_BOOT] = mt.boot_active;
|
||
in[ECAT_MOTW_OFF_MAGIC] = kEcatMotLayoutMagic;
|
||
wr32le(&in[ECAT_MOTW_OFF_GLOBAL_SEQ], mt.global_seq);
|
||
|
||
uint8_t w = 0U;
|
||
uint16_t newMask = 0U;
|
||
for (uint8_t i = 0U; i < n; ++i) {
|
||
const MotorSlot &s = *picks[i];
|
||
if ((s.wr_seq & 1U) != 0U) {
|
||
continue; /* 写入中：本拍省略，下拍自愈 */
|
||
}
|
||
__DMB();
|
||
|
||
uint8_t *p = &in[ECAT_MOTW_OFF_SLOTS + (uint16_t)w * kEcatMotSlotSize];
|
||
p[ECAT_MOT_OFF_NODE] = nodes[i];
|
||
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);
|
||
|
||
s_motPubSeq[nodes[i]] = s.seq;
|
||
s_motPubOnline[nodes[i]] = s.online;
|
||
newMask |= (uint16_t)(1U << nodes[i]);
|
||
++w;
|
||
}
|
||
|
||
in[ECAT_MOTW_OFF_COUNT] = w;
|
||
|
||
/* 更新影子：下次集合+seq 未变时整段复用 */
|
||
memcpy(s_motShadow, &in[ECAT_MOTW_OFF_COUNT], motRegionLen(w));
|
||
s_motPubMask = newMask;
|
||
s_motPubCount = w;
|
||
s_motShadowValid = true;
|
||
return w;
|
||
}
|
||
|
||
bool EthercatCanBridge::peekOutput(const uint8_t *head)
|
||
{
|
||
/* 输出探针：12B 头里 SEQ@0 / FLAGS。seq 无变化沿且无清统计请求时返回 false，
|
||
* PDO_OutputMapping 只补读 SM2 末字，跳过 384B 全量读。
|
||
* 恒 true 会让 8kHz 每拍都全量读，单拍被 PRAM 打爆（6kHz 即掉速）。 */
|
||
if (head == nullptr) {
|
||
return false;
|
||
}
|
||
const uint8_t seq = head[ECAT_CAN_TX_OFF_SEQ];
|
||
|
||
if (!s_seqPrimed) {
|
||
s_seqPrimed = true;
|
||
s_lastSeq = seq;
|
||
s_coldStartPending = true;
|
||
return true; /* 冷启动首包强制全量读，避免漏掉首帧命令 */
|
||
}
|
||
|
||
if (seq != s_lastSeq) {
|
||
return true;
|
||
}
|
||
return (head[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U;
|
||
}
|
||
|
||
void EthercatCanBridge::resetSequenceState()
|
||
{
|
||
s_seqPrimed = false;
|
||
s_lastSeq = 0U;
|
||
s_coldStartPending = false;
|
||
}
|
||
|
||
void EthercatCanBridge::processOutput(const uint8_t *out)
|
||
{
|
||
if (out == nullptr) {
|
||
return;
|
||
}
|
||
|
||
const uint8_t seq = out[ECAT_CAN_TX_OFF_SEQ];
|
||
s_outSeenSeq = seq; /* 诊断：processOutput 本拍看到的输出 seq */
|
||
|
||
if (!s_seqPrimed) {
|
||
s_seqPrimed = true;
|
||
s_coldStartPending = true;
|
||
}
|
||
|
||
/* 无论 seq 是否变化，优先处理非 seq 驱动的命令（FLAGS bit0 清统计） */
|
||
if ((out[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U) {
|
||
s_txCount = 0U;
|
||
s_dropCount = 0U;
|
||
s_rxCount = 0U;
|
||
s_rx00ffCount = 0U;
|
||
s_rxPublished = true;
|
||
}
|
||
|
||
const uint8_t dlcRaw = out[ECAT_CAN_TX_OFF_DLC];
|
||
const uint8_t batchCount = out[ECAT_TXB_OFF_COUNT];
|
||
const bool hasPayload = (dlcRaw > 0U) || (batchCount > 0U);
|
||
|
||
bool executeCommand = false;
|
||
if (s_coldStartPending) {
|
||
s_coldStartPending = false;
|
||
/* 冷启动：有载荷就发（不依赖 s_lastSeq 是否已被探针碰过） */
|
||
if (hasPayload) {
|
||
executeCommand = true;
|
||
} else {
|
||
s_lastSeq = seq;
|
||
return;
|
||
}
|
||
} else if (seq != s_lastSeq) {
|
||
executeCommand = true;
|
||
} else if (hasPayload && seq != s_lastSeq) {
|
||
executeCommand = true;
|
||
}
|
||
|
||
if (!executeCommand) {
|
||
return;
|
||
}
|
||
s_lastSeq = seq;
|
||
|
||
const uint8_t ch = out[ECAT_CAN_TX_OFF_CH];
|
||
uint8_t dlc = dlcRaw;
|
||
if (dlc > kEcatCanTxDataMax) {
|
||
dlc = kEcatCanTxDataMax;
|
||
}
|
||
|
||
if (dlc > 0U) {
|
||
if (!CanFd::isChannelEnabled(ch)) {
|
||
s_lastTxResult = (uint8_t)CanFd::CanTxResult::kErrInvalid;
|
||
} else if (MotorOtaRelay::isChannelLocked(ch)) {
|
||
/* 同 sendBatch：OTA 独占通道，周期运动帧不投邮箱（0xE7 上报主站）。 */
|
||
s_lastTxResult = (uint8_t)CanFd::CanTxResult::kErrOtaLocked;
|
||
} else {
|
||
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;
|
||
s_lastTxId = rd32le(&out[ECAT_CAN_TX_OFF_ID]);
|
||
}
|
||
}
|
||
}
|
||
|
||
(void)sendBatch(out);
|
||
}
|
||
|
||
void EthercatCanBridge::buildInput(uint8_t *in)
|
||
{
|
||
if (in == nullptr) {
|
||
return;
|
||
}
|
||
/* 不整包 memset(600)：8kHz 下每拍 Process 都会走到这里，600B 清零
|
||
* 与后续覆盖写重复。各段自己写满有效域；未用槽由 fillMotorRound
|
||
* 按 count 截断，主站只迭代 count，空洞区可为残值。 */
|
||
|
||
/* ── 段 1（0..31）：桥状态 + 短快照 ── */
|
||
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);
|
||
wr32le(&in[ECAT_CAN_RX_OFF_TXTXID], s_lastTxId);
|
||
wr16le(&in[ECAT_CAN_RX_OFF_RX00FF], s_rx00ffCount);
|
||
|
||
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);
|
||
memcpy(&in[ECAT_CAN_RX_OFF_DATA], s_rxData, kEcatCanRxDataMax);
|
||
s_rxPublished = true;
|
||
} else {
|
||
in[ECAT_CAN_RX_OFF_FLAGS] = 0U;
|
||
in[ECAT_CAN_RX_OFF_CH] = 0U;
|
||
in[ECAT_CAN_RX_OFF_DLC] = 0U;
|
||
}
|
||
|
||
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..543）：电机整轮密排 ── */
|
||
(void)fillMotorRound(in);
|
||
|
||
/* ── 段 3（544..583）：长帧快照（通用，不专宠版本） ──
|
||
* 语义 =「最近一帧非 STATUS 长帧」：CONFIG/版本/节点ID 谁后到谁上。
|
||
* STATUS(0x0100) 由段2 密排承载，不进本槽（否则 48B STATUS 会淹没一切）。
|
||
* 例外：仅当刚收到 0x00FF 的短粘滞窗口内，优先摆版本应答，
|
||
* 给主站一个稳定采样窗；窗口结束后立刻交还给 CONFIG 等，避免其它长帧失效。 */
|
||
if (xrxStable) {
|
||
const uint32_t nowMs = SysTickTimer::getMs();
|
||
const bool stickyLive = s_verValid && static_cast<int32_t>(nowMs - s_verHoldUntilMs) < 0;
|
||
/* 段3 RSVD@547：processOutput 看到的输出 seq（诊断用，不参与解析） */
|
||
in[ECAT_XRX_OFF_RSVD] = s_outSeenSeq;
|
||
if (stickyLive) {
|
||
in[ECAT_XRX_OFF_CH] = s_verCh;
|
||
in[ECAT_XRX_OFF_DLC] = s_verDlc;
|
||
in[ECAT_XRX_OFF_FLAGS] = 0x01U;
|
||
wr32le(&in[ECAT_XRX_OFF_ID], s_verId);
|
||
memcpy(&in[ECAT_XRX_OFF_DATA], s_verData, kEcatCanXRxDataMax);
|
||
} else if (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);
|
||
}
|
||
}
|
||
|
||
/* 段 5 由 EthercatPdo::fillDiagTail 写。 */
|
||
}
|
||
|
||
void EthercatCanBridge::onCanFrame(uint8_t channel, uint32_t id,
|
||
const uint8_t *data, uint8_t length)
|
||
{
|
||
if (data == nullptr) {
|
||
return;
|
||
}
|
||
|
||
/* STATUS(0x0100) 由 MotorState 段2 密排承载，不写入 xrx。
|
||
* 原注释本就如此约定；实现曾漏过滤，导致高频 STATUS 盖掉 CONFIG/版本。 */
|
||
if (canSubId(id) == 0x0100U) {
|
||
return;
|
||
}
|
||
|
||
uint8_t copyLen = length;
|
||
if (copyLen > kEcatCanXRxDataMax) {
|
||
copyLen = kEcatCanXRxDataMax;
|
||
}
|
||
|
||
/* ── 快照 seqlock：奇数=正在写，消费者本拍跳过段 1/段 3 ── */
|
||
++s_xrxSeq;
|
||
__DMB();
|
||
|
||
if (s_rxValid && !s_rxPublished) {
|
||
++s_dropCount;
|
||
}
|
||
|
||
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;
|
||
s_rxPublished = false;
|
||
|
||
if (canSubId(id) == 0x00FFU) {
|
||
if (s_rx00ffCount < 0xFFFFU) {
|
||
++s_rx00ffCount;
|
||
}
|
||
}
|
||
|
||
/* 版本应答：只做短粘滞，供主站在窗口内采到；不长期霸占 xrx */
|
||
if (canSubId(id) == 0x00FFU) {
|
||
s_verCh = channel;
|
||
s_verDlc = length;
|
||
s_verId = id;
|
||
memcpy(s_verData, data, copyLen);
|
||
if (copyLen < kEcatCanXRxDataMax) {
|
||
memset(&s_verData[copyLen], 0, (size_t)(kEcatCanXRxDataMax - copyLen));
|
||
}
|
||
s_verHoldUntilMs = SysTickTimer::getMs() + kVerHoldMs;
|
||
s_verValid = true;
|
||
}
|
||
|
||
__DMB();
|
||
++s_xrxSeq;
|
||
}
|
||
|
||
extern "C" {
|
||
|
||
void EcatCan_ProcessOutput(const uint8_t *out)
|
||
{
|
||
EthercatCanBridge::processOutput(out);
|
||
}
|
||
|
||
void EcatCan_BuildInput(uint8_t *in)
|
||
{
|
||
EthercatCanBridge::buildInput(in);
|
||
}
|
||
|
||
void EcatCan_ResetSequenceState(void)
|
||
{
|
||
EthercatCanBridge::resetSequenceState();
|
||
}
|
||
|
||
} /* extern "C" */
|