DCU_APP/GD32E508_DCU_APP/device/motor_state.cpp

/**
 * @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