Simba-OmniInfantry/底盘/modules/comms/referee.c

/* =============================================================================
 * referee.c - 裁判系统协议解析(0x0201 机器人状态 / 0x0202 实时功率 /
 *             0x0304 自定义控制器键鼠)
 * 输入:  referee_parse_byte() 流式喂字节(USART6 整帧回调里循环调用)
 * 校验:  CRC8(多项式0x31,初值0xFF) / CRC16(0x1021,0xFFFF),位运算实现,
 *        与 legacy 查表算法等价
 * 字段偏移与 legacy 逐一对应:
 *   0x0201: data[0]=robot_id, data[10..11]=功率上限, data[12]&0x04=发射机构上电
 *   0x0202: data[4..7]=实时功率(float), data[8..9]=缓冲能量(J)
 *   0x0304: 12 字节键鼠数据 -> keyboard 钩子(仅云台板注册)
 * 解析在接收中断内完成,任务层只读 referee_data_t 全局量
 * ========================================================================== */
#include "referee.h"
#include "main.h"

/* 帧结构: SOF(1) data_len(2) seq(1) CRC8(1) | cmd_id(2) data(n) CRC16(2)
 * 校验算法与 DJI 裁判系统协议一致:
 *   CRC8  多项式 0x31,初值 0xFF,输入输出不反转
 *   CRC16 多项式 0x1021,初值 0xFFFF,输入输出不反转 */

#define REF_FRAME_MAX_LEN 160

static uint8_t crc8_update(uint8_t crc, uint8_t data)
{
    crc ^= data;
    for (uint8_t i = 0; i < 8; i++)
        crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x31) : (uint8_t)(crc << 1);
    return crc;
}

static uint16_t crc16_update(uint16_t crc, uint8_t data)
{
    crc ^= (uint16_t)data << 8;
    for (uint8_t i = 0; i < 8; i++)
        crc = (crc & 0x8000) ? (uint16_t)((crc << 1) ^ 0x1021) : (uint16_t)(crc << 1);
    return crc;
}

static uint8_t verify_crc8(const uint8_t * data, uint16_t len)
{
    uint8_t crc = 0xFF;
    for (uint16_t i = 0; i < len - 1; i++)
        crc = crc8_update(crc, data[i]);
    return crc == data[len - 1];
}

static uint8_t verify_crc16(const uint8_t * data, uint16_t len)
{
    uint16_t crc = 0xFFFF;
    for (uint16_t i = 0; i < len - 2; i++)
        crc = crc16_update(crc, data[i]);
    return (uint8_t)(crc & 0xFF) == data[len - 2] && (uint8_t)(crc >> 8) == data[len - 1];
}

referee_data_t referee = {
    .robot_id = 0,
    .shooter_power_on = 1,
    .chassis_power_limit = 60,
    .chassis_power = 0,
    .buffer_energy = 60.0f,
    .link_status = 0,
    .last_update_tick = 0,
};

static referee_keyboard_hook_f keyboard_hook = NULL;

void referee_set_keyboard_hook(referee_keyboard_hook_f hook)
{
    keyboard_hook = hook;
}

/* 解析状态机 */
typedef enum
{
    REF_STATE_SOF = 0,
    REF_STATE_LEN_LOW,
    REF_STATE_LEN_HIGH,
    REF_STATE_SEQ,
    REF_STATE_HEADER_CRC8,
    REF_STATE_CMD_LOW,
    REF_STATE_CMD_HIGH,
    REF_STATE_DATA,
    REF_STATE_CRC16_LOW,
    REF_STATE_CRC16_HIGH,
} ref_state_e;

static ref_state_e ref_state = REF_STATE_SOF;
static uint16_t ref_data_len = 0;
static uint16_t ref_cmd_id = 0;
static uint16_t ref_data_index = 0;
static uint8_t ref_frame_buf[REF_FRAME_MAX_LEN];
static uint16_t ref_frame_index = 0;

static void referee_dispatch(uint16_t cmd_id, const uint8_t * data, uint16_t len)
{
    switch (cmd_id)
    {
        case 0x0201:  // 机器人状态(与 legacy 偏移一致)
            if (len >= 13)
            {
                referee.robot_id = data[0];
                referee.chassis_power_limit = (uint16_t)(data[10] | (data[11] << 8));
                referee.shooter_power_on = (data[12] & 0x04) >> 2;
                referee.last_update_tick = HAL_GetTick();
                referee.link_status = 1;
            }
            break;

        case 0x0202:  // 实时功率(与 legacy 偏移一致)
            if (len >= 10)
            {
                float power;
                memcpy(&power, &data[4], 4);
                referee.chassis_power = power;
                referee.buffer_energy = (float)((uint16_t)(data[8] | (data[9] << 8)));
                referee.last_update_tick = HAL_GetTick();
                referee.link_status = 1;
            }
            break;

        case 0x0304:  // 自定义控制器(经图传链路转发)
            if (len >= 12 && keyboard_hook != NULL)
                keyboard_hook(data);
            referee.last_update_tick = HAL_GetTick();
            referee.link_status = 1;
            break;

        default:
            break;
    }
}

void referee_parse_byte(uint8_t byte)
{
    switch (ref_state)
    {
        case REF_STATE_SOF:
            if (byte == 0xA5)
            {
                ref_frame_index = 0;
                ref_frame_buf[ref_frame_index++] = byte;
                ref_state = REF_STATE_LEN_LOW;
            }
            break;

        case REF_STATE_LEN_LOW:
            ref_data_len = byte;
            ref_frame_buf[ref_frame_index++] = byte;
            ref_state = REF_STATE_LEN_HIGH;
            break;

        case REF_STATE_LEN_HIGH:
            ref_data_len |= (uint16_t)byte << 8;
            ref_frame_buf[ref_frame_index++] = byte;
            if (ref_data_len > REF_FRAME_MAX_LEN - 9)  // 帧过长,重新找帧头
                ref_state = REF_STATE_SOF;
            else
                ref_state = REF_STATE_SEQ;
            break;

        case REF_STATE_SEQ:
            ref_frame_buf[ref_frame_index++] = byte;
            ref_state = REF_STATE_HEADER_CRC8;
            break;

        case REF_STATE_HEADER_CRC8:
            ref_frame_buf[ref_frame_index++] = byte;
            if (verify_crc8(ref_frame_buf, 5))
                ref_state = REF_STATE_CMD_LOW;
            else
                ref_state = REF_STATE_SOF;
            break;

        case REF_STATE_CMD_LOW:
            ref_cmd_id = byte;
            ref_frame_buf[ref_frame_index++] = byte;
            ref_state = REF_STATE_CMD_HIGH;
            break;

        case REF_STATE_CMD_HIGH:
            ref_cmd_id |= (uint16_t)byte << 8;
            ref_frame_buf[ref_frame_index++] = byte;
            ref_data_index = 0;
            if (ref_data_len == 0)
                ref_state = REF_STATE_CRC16_LOW;
            else
                ref_state = REF_STATE_DATA;
            break;

        case REF_STATE_DATA:
            ref_frame_buf[ref_frame_index++] = byte;
            if (++ref_data_index >= ref_data_len)
                ref_state = REF_STATE_CRC16_LOW;
            break;

        case REF_STATE_CRC16_LOW:
            ref_frame_buf[ref_frame_index++] = byte;
            ref_state = REF_STATE_CRC16_HIGH;
            break;

        case REF_STATE_CRC16_HIGH:
            ref_frame_buf[ref_frame_index++] = byte;
            if (verify_crc16(ref_frame_buf, ref_frame_index))
                referee_dispatch(ref_cmd_id, &ref_frame_buf[7], ref_data_len);
            ref_state = REF_STATE_SOF;
            break;

        default:
            ref_state = REF_STATE_SOF;
            break;
    }
}

void referee_parse_bytes(const uint8_t * data, uint16_t len)
{
    while (len--)
        referee_parse_byte(*data++);
}