LSMushui
a4da5ad17c
- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
219 lines
6.7 KiB
C
219 lines
6.7 KiB
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++);
|
|
}
|