lyf
267a659242
键鼠控制(此前 WASD 无反应): - 云台 robot_cmd_task:0x0304 与 USART3 帧内键鼠合并为共享视图 keyboard_merged(0x0304 优先); 键鼠模式由帧内键鼠动作进入,动云台左摇杆退出 - c_comm_task/c_comm:键鼠在控时底盘摇杆由 WASD+鼠标合成,改读共享视图(此前只认 0x0304) - dr16/调试输出:新增帧内键鼠原始字节诊断(key_lo/key_hi/mouse_*/s1/frames) 自定义UI(底盘板经裁判系统 USART6 绘制到选手端): - 新增 底盘/modules/comms/referee_ui.c|h:15 字节图形组包(带编译期断言), v5 卡片式 HUD = 俯仰梯(pitch) + 罗盘刻度带(底盘朝向) + 预估底盘功率 + 超级电容 + 摩擦轮 L/R 转速 + 小陀螺状态,yaw 在刻度带右侧;5Hz 刷新,约 50 图元/8 包 - referee.c 新增 referee_send_frame()(cmd 0x0301 机器人交互数据,CRC8+CRC16,USART6 TX DMA) - 新增 CToC 0x1B9:云台把 pitch 与摩擦轮转速带给底盘(底盘本地没有这两个量) - 版式避开官方保留区:顶部比分条/左下小地图/右下战车模块/中部两块官方保留 验证:云台 Code=99540 RO=1764 RW=420 ZI=62572;底盘 Code=99452 RO=1928 RW=416 ZI=62584;均 0 错误 0 警告。 未做实机验证(当前无板/无图传);UI 协议常量(UI_RECEIVER_ID/UI_SEND_HZ)与版式坐标集中在 referee_ui.h 宏区,便于按协议 PDF 一处校正。
264 lines
8.3 KiB
C
264 lines
8.3 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++);
|
|
}
|
|
|
|
/* ============================================================================
|
|
* 发送:裁判系统帧(机器人 -> 裁判系统 -> 选手端)
|
|
* 帧: A5 | data_length(2,LE,=payload 字节数,不含 cmd_id) | seq | CRC8
|
|
* | cmd_id(2,LE) | data | CRC16(2,LE,低字节先)
|
|
* CRC8 覆盖前 4 字节;CRC16 覆盖除末 2 字节外全部。
|
|
* 自定义UI 走 cmd 0x0301(payload 首 6 字节为 data_cmd_id/sender_ID/receiver_ID)。
|
|
* USART6 TX DMA:静态缓冲,发送期间不得被覆盖;UI 刷新率 5Hz 远低于链路能力。
|
|
* ==========================================================================*/
|
|
#include "bsp_usart.h"
|
|
|
|
static uint8_t ref_tx_seq = 0;
|
|
|
|
void referee_send_frame(uint16_t cmd_id, const uint8_t * data, uint16_t len)
|
|
{
|
|
static uint8_t frame[REF_FRAME_MAX_LEN];
|
|
uint16_t i = 0;
|
|
|
|
if (len > (uint16_t)(REF_FRAME_MAX_LEN - 9) || data == NULL)
|
|
return;
|
|
|
|
frame[i++] = 0xA5;
|
|
frame[i++] = (uint8_t)(len & 0xFF);
|
|
frame[i++] = (uint8_t)(len >> 8);
|
|
frame[i++] = ref_tx_seq++;
|
|
|
|
uint8_t crc8 = 0xFF;
|
|
for (uint16_t k = 0; k < 4; k++)
|
|
crc8 = crc8_update(crc8, frame[k]);
|
|
frame[i++] = crc8;
|
|
|
|
frame[i++] = (uint8_t)(cmd_id & 0xFF);
|
|
frame[i++] = (uint8_t)(cmd_id >> 8);
|
|
|
|
for (uint16_t k = 0; k < len; k++)
|
|
frame[i++] = data[k];
|
|
|
|
uint16_t crc16 = 0xFFFF;
|
|
for (uint16_t k = 0; k < i; k++)
|
|
crc16 = crc16_update(crc16, frame[k]);
|
|
frame[i++] = (uint8_t)(crc16 & 0xFF);
|
|
frame[i++] = (uint8_t)(crc16 >> 8);
|
|
|
|
usart6_tx_dma_enable(frame, i);
|
|
}
|