144 lines
4.3 KiB
C
144 lines
4.3 KiB
C
#include "device/ai.h"
|
||
#include "device/device.h"
|
||
#include "bsp/uart.h"
|
||
#include "component/crc16.h"
|
||
#include "device/referee.h"
|
||
|
||
/* ---- 接收缓冲 ---- */
|
||
static uint8_t ai_rx_buf[64]; /* DMA 接收缓冲，略大于 PackageAI_t */
|
||
static AI_result_t ai_parsed; /* 解析后的结果 */
|
||
static volatile uint8_t ai_data_ready = 0;
|
||
|
||
static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
|
||
uint16_t *crc = NULL;
|
||
|
||
if (raw == NULL || len <= sizeof(uint16_t)) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
raw[0] = id;
|
||
crc = (uint16_t *)(void *)(raw + len - sizeof(uint16_t));
|
||
*crc = CRC16_Calc(raw, len - sizeof(uint16_t), CRC16_INIT);
|
||
|
||
if (CRC16_Verify(raw, len) != true) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/* IDLE LINE 回调 —— 上位机发完一帧后总线空闲触发 */
|
||
static void AI_IdleLineCallback(void) {
|
||
UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_1);
|
||
|
||
/* 停止DMA，计算实际收到的字节数 */
|
||
HAL_UART_DMAStop(huart);
|
||
uint16_t rx_len = sizeof(ai_rx_buf) - __HAL_DMA_GET_COUNTER(huart->hdmarx);
|
||
|
||
/* 长度匹配才解析 */
|
||
if (rx_len == sizeof(PackageAI_t)) {
|
||
PackageAI_t *pkt = (PackageAI_t *)ai_rx_buf;
|
||
|
||
if (pkt->id == ID_AI && CRC16_Verify((uint8_t *)pkt, sizeof(PackageAI_t))) {
|
||
ai_parsed.mode = pkt->data.mode;
|
||
ai_parsed.gimbal_t.setpoint.yaw = pkt->data.yaw;
|
||
ai_parsed.gimbal_t.vel.yaw = pkt->data.yaw_vel;
|
||
ai_parsed.gimbal_t.accl.yaw = pkt->data.yaw_acc;
|
||
ai_parsed.gimbal_t.setpoint.pit = pkt->data.pitch;
|
||
ai_parsed.gimbal_t.vel.pit = pkt->data.pitch_vel;
|
||
ai_parsed.gimbal_t.accl.pit = pkt->data.pitch_acc;
|
||
ai_parsed.chassis_t.Vx = pkt->data.vx;
|
||
ai_parsed.chassis_t.Vy = pkt->data.vy;
|
||
ai_parsed.chassis_t.Vw = pkt->data.wz;
|
||
ai_data_ready = 1;
|
||
}
|
||
}
|
||
|
||
/* 重启DMA接收，等待下一帧 */
|
||
BSP_UART_Receive(BSP_UART_1, ai_rx_buf, sizeof(ai_rx_buf), true);
|
||
}
|
||
|
||
/**
|
||
* @brief 初始化AI通信，注册IDLE回调并启动DMA接收
|
||
*/
|
||
int8_t AI_Init(void) {
|
||
UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_1);
|
||
|
||
/* 注册IDLE LINE回调 */
|
||
BSP_UART_RegisterCallback(BSP_UART_1, BSP_UART_IDLE_LINE_CB,
|
||
(void (*)(void))AI_IdleLineCallback);
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_1, BSP_UART_IDLE_LINE_CB, (void (*)(void))AI_IdleLineCallback);
|
||
|
||
/* 启动第一次DMA接收 */
|
||
BSP_UART_Receive(BSP_UART_1, ai_rx_buf, sizeof(ai_rx_buf), true);
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取最新的AI数据（非阻塞，task循环中调用）
|
||
* @return DEVICE_OK=有新数据已拷贝, DEVICE_ERR=无新数据
|
||
*/
|
||
int8_t AI_GetLatest(AI_result_t *result) {
|
||
if (result == NULL) {
|
||
return DEVICE_ERR;
|
||
}
|
||
if (!ai_data_ready) {
|
||
return DEVICE_ERR;
|
||
}
|
||
*result = ai_parsed;
|
||
ai_data_ready = 0;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/* ---- 以下发送函数不变 ---- */
|
||
|
||
int8_t MCU_Send(PackageMCU_t *mcu, Gimbal_feedback_t *motor, AHRS_Quaternion_t *quat) {
|
||
(void)quat;
|
||
if (mcu == NULL || motor == NULL) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
mcu->data.mode = 1;
|
||
mcu->data.q[0] = motor->imu.quat.q0;
|
||
mcu->data.q[1] = motor->imu.quat.q1;
|
||
mcu->data.q[2] = motor->imu.quat.q2;
|
||
mcu->data.q[3] = motor->imu.quat.q3;
|
||
mcu->data.yaw = motor->imu.eulr.yaw;
|
||
mcu->data.yaw_vel = motor->imu.gyro.z;
|
||
mcu->data.pitch = motor->imu.eulr.rol;
|
||
mcu->data.pitch_vel = motor->imu.gyro.x;
|
||
mcu->data.bullet_speed = 22;
|
||
|
||
return Package_BuildAndVerify((uint8_t *)mcu, sizeof(*mcu), ID_MCU);
|
||
}
|
||
|
||
int8_t REF_Send(PackageReferee_t *referee, Referee_RobotStatus_t *robot_status, Referee_GameStatus_t *game_status) {
|
||
if (referee == NULL || robot_status == NULL || game_status == NULL) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
referee->data.remain_hp = robot_status->current_HP;
|
||
referee->data.game_progress = game_status->game_progress & 0x0F;
|
||
referee->data.stage_remain_time = game_status->stage_remain_time;
|
||
|
||
return Package_BuildAndVerify((uint8_t *)referee, sizeof(*referee), ID_REF);
|
||
}
|
||
|
||
int8_t REF_StartSend(PackageReferee_t *referee) {
|
||
if (referee == NULL) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
if (BSP_UART_Transmit(BSP_UART_1, (uint8_t *)referee, sizeof(*referee), true) == HAL_OK)
|
||
return DEVICE_OK;
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
int8_t MCU_StartSend(PackageMCU_t *mcu) {
|
||
if (BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true) == HAL_OK)
|
||
return DEVICE_OK;
|
||
return DEVICE_ERR;
|
||
}
|
||
|