cbb
0cd3d9bed2
回退1.6.0引入的LAN9252 PRAM批量读写优化，因其AVAIL_CNT计数器对短读存在语义问题， 导致邮箱短读和探针传输时出现FIFO陈旧数据损坏问题。 同时修复EtherCAT-CAN桥中的多个竞态条件： - 修复消费者在fillRxBatch中直接访问共享ring buffer导致的数据混淆问题 - 修复生产者-消费者间tail指针发布时机不当造成的槽位覆盖竞态 - 修复WIN窗口基址更新位置错误导致电机状态窗口被锁定的问题 - 修复buildInput中断上下文与主循环间的数据竞争和内存撕裂问题 - 添加STATUS帧过滤机制减少批量区压力并解决队列饱和问题 此外还增加了多个诊断计数器用于监控撕裂、溢出和拼接错误，并调整了 输入PDO段布局和大小参数以支持新的48字节STATUS格式。 这些修改解决了上位机观察到的帧标签错误、电机状态丢失和数据拼接问题。
198 lines
7.2 KiB
C++
198 lines
7.2 KiB
C++
/**
|
||
* @file motor_state.cpp
|
||
* @brief 多电机状态表实现 — CAN STATUS 帧识别、内存缓存、SPI 推送序列化。
|
||
*
|
||
* 本模块的函数只在主循环上下文调用（taskCanRxParse / taskHeartbeat / pushStep），
|
||
* 但消费者不止主循环：EtherCAT 的 buildInput 跑在 PDI_Isr，会抢占 STATUS 写侧，
|
||
* 故 STATUS 的 seq/data 由 MotorSlot.wr_seq（seqlock）保护，见 onCanFrame。
|
||
* SPI 侧的 buildPayload 与写侧同上下文，天然无此问题。
|
||
*/
|
||
|
||
#include "device/motor_state.h"
|
||
#include "device/spi/app_spi_command_defs.h"
|
||
#include "main.h"
|
||
#include <cstring>
|
||
|
||
/* ── 全局状态表实例 ── */
|
||
static MotorStateTable s_motorTable = {};
|
||
|
||
namespace MotorState {
|
||
|
||
MotorStateTable &table()
|
||
{
|
||
return s_motorTable;
|
||
}
|
||
|
||
void onCanFrame(uint8_t channel, uint32_t id, const uint8_t *data, uint8_t len)
|
||
{
|
||
/* BOOT 心跳 0x103（固定 ID，不带节点号）：电机在 Bootloader IDLE 每 ~10ms 发一次。
|
||
* 记录最近收到时刻与通道，供网页显示独立 BOOT 状态（不归属任何 Nx）。
|
||
* 注意：必须在 STATUS 掩码过滤之前拦截，否则 0x103 会被下面的掩码判断丢弃。 */
|
||
if (id == kOtaBootHeartbeatId) {
|
||
s_motorTable.boot_hb_sec = g_app_uptime_sec;
|
||
s_motorTable.boot_channel = channel;
|
||
s_motorTable.boot_active = 1U;
|
||
/* 0x103 仅更新 boot_active 状态，不再串口打印（高频心跳会刷屏）。 */
|
||
return;
|
||
}
|
||
|
||
const uint32_t masked = id & kMotorStatusIdMask;
|
||
|
||
/* STATUS 帧 0x000N0100 (DLC 48, 实时值，字节 32~35 = InitStatus) */
|
||
if (masked == kMotorStatusIdMatch) {
|
||
const uint8_t node = static_cast<uint8_t>((id >> 16) & 0x0FU);
|
||
if (node < 1U || node > kMotorMaxNodes || channel >= kMotorChannels) {
|
||
return;
|
||
}
|
||
|
||
MotorSlot &slot = s_motorTable.slots[(node - 1U) * kMotorChannels + channel];
|
||
|
||
/* seqlock 写侧（1.6.5）：奇数=本槽 seq/data 正在改。EtherCAT 的 buildInput
|
||
* 跑在 PDI_Isr，会抢占这里；抢占落在下面那次 memcpy 中途就会发布
|
||
* "新 seq + 半新半旧 48B"——两帧字节拼成一个 STATUS，边界若落在 float 的
|
||
* 符号/指数字节上就是网页上的离谱值。缺的不是顺序而是让消费者能认出
|
||
* "这一段没写完"的标记。 */
|
||
++slot.wr_seq; /* → 奇数 */
|
||
__DMB(); /* 普通写不得重排到奇/偶两次自增之外 */
|
||
|
||
slot.node_id = node;
|
||
slot.channel = channel;
|
||
slot.online = 1U;
|
||
slot.dlc = (len <= kMotorStatusDlc) ? len : kMotorStatusDlc;
|
||
slot.rx_count++;
|
||
slot.last_rx_sec = g_app_uptime_sec;
|
||
slot.seq = ++s_motorTable.global_seq;
|
||
|
||
std::memcpy(slot.data, data, slot.dlc);
|
||
if (slot.dlc < kMotorStatusDlc) {
|
||
std::memset(&slot.data[slot.dlc], 0, kMotorStatusDlc - slot.dlc);
|
||
}
|
||
|
||
__DMB();
|
||
++slot.wr_seq; /* → 偶数：本槽整体一致 */
|
||
return;
|
||
}
|
||
|
||
/* CONFIG 帧 0x000N0101 (DLC 24, 目标值回报) */
|
||
if (masked == kMotorConfigIdMatch) {
|
||
const uint8_t node = static_cast<uint8_t>((id >> 16) & 0x0FU);
|
||
if (node < 1U || node > kMotorMaxNodes || channel >= kMotorChannels) {
|
||
return;
|
||
}
|
||
|
||
MotorSlot &slot = s_motorTable.slots[(node - 1U) * kMotorChannels + channel];
|
||
|
||
/* CONFIG 帧不更新 online 状态（仅 STATUS 帧维护在线检测） */
|
||
slot.node_id = node; /* 确保槽位不空 */
|
||
slot.channel = channel;
|
||
slot.config_rx_count++;
|
||
slot.config_seq = ++s_motorTable.global_seq;
|
||
|
||
const uint8_t copyLen = (len <= kMotorConfigDlc) ? len : kMotorConfigDlc;
|
||
std::memcpy(slot.config_data, data, copyLen);
|
||
if (copyLen < kMotorConfigDlc) {
|
||
std::memset(&slot.config_data[copyLen], 0, kMotorConfigDlc - copyLen);
|
||
}
|
||
return;
|
||
}
|
||
}
|
||
|
||
void checkOffline()
|
||
{
|
||
const uint32_t now = g_app_uptime_sec;
|
||
uint8_t active = 0U;
|
||
|
||
for (uint8_t i = 0U; i < kMotorSlotCount; ++i) {
|
||
MotorSlot &slot = s_motorTable.slots[i];
|
||
if (slot.node_id == 0U) {
|
||
continue; /* 空槽 */
|
||
}
|
||
if ((now - slot.last_rx_sec) >= kMotorOfflineSec) {
|
||
slot.online = 0U;
|
||
}
|
||
if (slot.online != 0U) {
|
||
active++;
|
||
}
|
||
}
|
||
s_motorTable.active_count = active;
|
||
|
||
/* BOOT 心跳超时：>kBootHeartbeatTimeoutSec 无 0x103 → 电机已退出 BOOT。 */
|
||
if (s_motorTable.boot_active != 0U &&
|
||
(now - s_motorTable.boot_hb_sec) >= kBootHeartbeatTimeoutSec) {
|
||
s_motorTable.boot_active = 0U;
|
||
}
|
||
}
|
||
|
||
uint16_t buildPayload(uint8_t page, uint8_t *buf, uint16_t maxLen)
|
||
{
|
||
if (page >= kMotorTotalPages || buf == nullptr) {
|
||
return 0U;
|
||
}
|
||
|
||
/* 需要的最小空间: header + 至少 1 entry */
|
||
const uint16_t needed = kMotorPageHeaderLen + kMotorEntrySize;
|
||
if (maxLen < needed) {
|
||
return 0U;
|
||
}
|
||
|
||
const uint8_t startIdx = static_cast<uint8_t>(page * kMotorEntriesPerPage);
|
||
uint8_t count = 0U;
|
||
|
||
/* Header (4B) */
|
||
buf[0] = page;
|
||
buf[1] = kMotorTotalPages;
|
||
buf[2] = 0U; /* entry_count, 稍后回填 */
|
||
/* bit[6:0]=在线电机数，bit7=BOOT 心跳(0x103)活跃标志（供网页显示独立 BOOT 状态）。
|
||
* active_count 上限 9，bit7 空闲可复用，旧解析方读到会多算 128，故 ESP32 侧需掩 0x7F。 */
|
||
buf[3] = static_cast<uint8_t>((s_motorTable.active_count & 0x7FU)
|
||
| (s_motorTable.boot_active ? kMotorActiveBootBit : 0U));
|
||
|
||
uint16_t offset = kMotorPageHeaderLen;
|
||
|
||
for (uint8_t i = 0U; i < kMotorEntriesPerPage; ++i) {
|
||
const uint8_t slotIdx = static_cast<uint8_t>(startIdx + i);
|
||
if (slotIdx >= kMotorSlotCount) {
|
||
break;
|
||
}
|
||
|
||
const MotorSlot &slot = s_motorTable.slots[slotIdx];
|
||
if (slot.node_id == 0U) {
|
||
continue; /* 跳过空槽 */
|
||
}
|
||
|
||
/* 检查剩余空间 */
|
||
if (offset + kMotorEntrySize > maxLen) {
|
||
break;
|
||
}
|
||
|
||
buf[offset + 0U] = slot.node_id;
|
||
buf[offset + 1U] = slot.channel;
|
||
buf[offset + 2U] = slot.online;
|
||
buf[offset + 3U] = slot.dlc;
|
||
writeU32Le(&buf[offset + 4U], slot.rx_count);
|
||
writeU32Le(&buf[offset + 8U], slot.seq);
|
||
std::memcpy(&buf[offset + 12U], slot.data, kMotorStatusDlc);
|
||
/* STATUS DLC 48 后，CONFIG 字段整体后移 16B */
|
||
writeU32Le(&buf[offset + 60U], slot.config_rx_count);
|
||
writeU32Le(&buf[offset + 64U], slot.config_seq);
|
||
std::memcpy(&buf[offset + 68U], slot.config_data, kMotorConfigDlc);
|
||
|
||
offset += kMotorEntrySize;
|
||
count++;
|
||
}
|
||
|
||
buf[2] = count;
|
||
return offset;
|
||
}
|
||
|
||
uint16_t setPushPeriodMs(uint16_t ms)
|
||
{
|
||
/* 2026-08 请求驱动改造：0xC5 主动推送已移除，周期设置不再生效。
|
||
* 仅钳制后回显，保留 0xFE 子命令协议兼容（ESP32 当前不发送）。 */
|
||
if (ms < kMotorPeriodMinMs) { ms = kMotorPeriodMinMs; }
|
||
if (ms > kMotorPeriodMaxMs) { ms = kMotorPeriodMaxMs; }
|
||
return ms;
|
||
}
|
||
|
||
} // namespace MotorState
|