194 lines
7.6 KiB
C
194 lines
7.6 KiB
C
/*
|
||
ai Task
|
||
功能：
|
||
1. 接收姿态任务 (atti_esti) 发送的四元数。
|
||
2. 直接读取全局云台(gimbal)与发射机构(shoot)状态，打包发送给上位机。
|
||
3. 解析上位机指令并转化为系统的云台与发射控制信号。
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include <stdint.h>
|
||
#include <string.h>
|
||
#include "bsp/uart.h"
|
||
#include "component/crc16.h"
|
||
#include "module/gimbal.h" /* 引用云台结构体获取反馈 */
|
||
#include "module/shoot.h" /* 引用发射机构结构体获取反馈 */
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
extern uint8_t AI_mode; // AI 模式变量，供其他模块查询当前AI控制模式
|
||
|
||
// MCU 数据结构（MCU -> 上位机）
|
||
typedef struct __attribute__((packed)) {
|
||
uint8_t head[2]; // {'M', 'R'}
|
||
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
|
||
float q[4]; // wxyz顺序
|
||
float yaw; // 偏航角
|
||
float yaw_vel; // 偏航角速度
|
||
float pitch; // 俯仰角
|
||
float pitch_vel; // 俯仰角速度
|
||
float bullet_speed; // 弹速
|
||
uint16_t bullet_count; // 子弹累计发送次数
|
||
uint16_t crc16;
|
||
} GimbalToVision_t;
|
||
|
||
// 确保结构体大小符合要求 (C11 标准)
|
||
#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
|
||
_Static_assert(sizeof(GimbalToVision_t) <= 64, "GimbalToVision_t size exceeds 64 bytes");
|
||
#endif
|
||
|
||
// AI 控制数据结构（上位机 -> MCU）
|
||
typedef struct __attribute__((packed)) {
|
||
uint8_t head[2]; // {'M', 'R'}
|
||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
|
||
float yaw; // 目标偏航角/偏移量
|
||
float yaw_vel; // 偏航角速度
|
||
float yaw_acc; // 偏航角加速度
|
||
float pitch; // 目标俯仰角/偏移量
|
||
float pitch_vel; // 俯仰角速度
|
||
float pitch_acc; // 俯仰角加速度
|
||
uint16_t crc16;
|
||
} VisionToGimbal_t;
|
||
|
||
#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
|
||
_Static_assert(sizeof(VisionToGimbal_t) <= 64, "VisionToGimbal_t size exceeds 64 bytes");
|
||
#endif
|
||
|
||
/* Private variables -------------------------------------------------------- */
|
||
static uint8_t ai_rx_buf[sizeof(VisionToGimbal_t)];
|
||
static VisionToGimbal_t ai_rx_data;
|
||
|
||
/* 声明外部全局变量，用于直接获取高频反馈数据 */
|
||
extern Gimbal_t gimbal;
|
||
extern Shoot_t shoot;
|
||
GimbalToVision_t tx_pkg;
|
||
/* USER FUNCTION BEGIN */
|
||
|
||
/**
|
||
* @brief AI串口接收完成回调函数
|
||
* @note 验证包头与CRC16校验，校验通过后更新控制指令，并重新开启DMA接收
|
||
*/
|
||
static void AI_RxCpltCallback(void) {
|
||
/* 检查包头是否为 'M' 和 'R' */
|
||
if (ai_rx_buf[0] == 'M' && ai_rx_buf[1] == 'R') {
|
||
/* 校验 CRC，确保数据包完整 */
|
||
if (CRC16_Verify(ai_rx_buf, sizeof(VisionToGimbal_t))) {
|
||
memcpy(&ai_rx_data, ai_rx_buf, sizeof(VisionToGimbal_t));
|
||
|
||
/* --- 务必初始化局部变量 --- */
|
||
Gimbal_Vision_t vision_data;
|
||
memset(&vision_data, 0, sizeof(Gimbal_Vision_t));
|
||
|
||
Shoot_CMD_t ai_shoot_cmd;
|
||
memset(&ai_shoot_cmd, 0, sizeof(Shoot_CMD_t)); // 防止野指针走火
|
||
|
||
/* --- 正确赋值视觉结构体 --- */
|
||
switch (ai_rx_data.mode) {
|
||
case 0: /* 不控制 */
|
||
vision_data.target_found = 0;
|
||
// ai_shoot_cmd.mode = SHOOT_MODE_STOP;
|
||
break;
|
||
case 1: /* 控制云台但不开火 */
|
||
vision_data.target_found = 1;
|
||
vision_data.yaw = ai_rx_data.yaw;
|
||
vision_data.pit = ai_rx_data.pitch;
|
||
vision_data.yaw_v_ff = ai_rx_data.yaw_vel;
|
||
vision_data.pit_v_ff = ai_rx_data.pitch_vel;
|
||
// ai_shoot_cmd.mode = SHOOT_MODE_READY;
|
||
break;
|
||
case 2: /* 控制云台且开火 */
|
||
vision_data.target_found = 1;
|
||
vision_data.yaw = ai_rx_data.yaw;
|
||
vision_data.pit = ai_rx_data.pitch;
|
||
vision_data.yaw_v_ff = ai_rx_data.yaw_vel;
|
||
vision_data.pit_v_ff = ai_rx_data.pitch_vel;
|
||
// ai_shoot_cmd.mode = SHOOT_MODE_FIRE;
|
||
break;
|
||
default:
|
||
vision_data.target_found = 0;
|
||
break;
|
||
}
|
||
|
||
/* 将视觉数据发送到专属队列，不与遥控器命令冲突 */
|
||
osMessageQueuePut(task_runtime.msgq.vision.data, &vision_data, 0, 0);
|
||
|
||
/* 将发射指令发送到发射队列 (确保枚举名称与shoot.h对应后解开注释) */
|
||
// osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &ai_shoot_cmd, 0, 0);
|
||
}
|
||
}
|
||
/* 重新开启DMA定长接收 */
|
||
BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(VisionToGimbal_t), true);
|
||
}
|
||
|
||
/**
|
||
* @brief 向上位机发送MCU状态数据
|
||
*/
|
||
static void AI_Send_MCU_Data(void) {
|
||
AHRS_Quaternion_t current_quat;
|
||
|
||
memset(&tx_pkg, 0, sizeof(GimbalToVision_t));
|
||
|
||
/* 写入包头 */
|
||
tx_pkg.head[0] = 'M';
|
||
tx_pkg.head[1] = 'R';
|
||
|
||
/* 1. 获取四元数 (从消息队列读取) */
|
||
if (osMessageQueueGet(task_runtime.msgq.ai.quat, ¤t_quat, NULL, 0) == osOK) {
|
||
tx_pkg.q[0] = current_quat.q0;
|
||
tx_pkg.q[1] = current_quat.q1;
|
||
tx_pkg.q[2] = current_quat.q2;
|
||
tx_pkg.q[3] = current_quat.q3;
|
||
}
|
||
|
||
/* 2. 获取云台状态 (直接读取全局 gimbal 结构体) */
|
||
tx_pkg.yaw = gimbal.feedback.imu.eulr.yaw;
|
||
tx_pkg.yaw_vel = gimbal.feedback.imu.gyro.z;
|
||
tx_pkg.pitch = gimbal.feedback.imu.eulr.rol; /* 电控中 Pitch 对应 IMU 的 Rol */
|
||
tx_pkg.pitch_vel = gimbal.feedback.imu.gyro.y;
|
||
|
||
/* 3. 获取发射机构与模式状态 (直接读取全局 shoot 结构体) */
|
||
/* 模式回传，取值为实际枚举映射 */
|
||
tx_pkg.mode = AI_mode; /* 直接回传当前AI模式，供上位机显示 */
|
||
|
||
/* 弹速反馈：暂用摩擦轮目标转速或估算转速代替。若接入裁判系统，应替换为真实弹速。 */
|
||
tx_pkg.bullet_speed = shoot.target_variable.target_rpm;
|
||
|
||
/* 弹量统计：如有独立计数变量，可进行赋值 */
|
||
tx_pkg.bullet_count = -1;
|
||
|
||
/* 4. 计算 CRC16（扣除末尾 2 字节） */
|
||
tx_pkg.crc16 = CRC16_Calc((const uint8_t *)&tx_pkg, sizeof(GimbalToVision_t) - 2, CRC16_INIT);
|
||
|
||
/* 5. 通过 DMA 发送至上位机 */
|
||
BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&tx_pkg, sizeof(GimbalToVision_t), true);
|
||
}
|
||
|
||
/* USER FUNCTION END */
|
||
|
||
void Task_ai(void *argument) {
|
||
(void)argument;
|
||
|
||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
|
||
osDelay(AI_INIT_DELAY);
|
||
uint32_t tick = osKernelGetTickCount();
|
||
|
||
/* USER CODE INIT BEGIN */
|
||
/* 注册串口接收完成回调并启动第一次DMA接收 */
|
||
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, AI_RxCpltCallback);
|
||
BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(VisionToGimbal_t), true);
|
||
/* USER CODE INIT END */
|
||
|
||
while (1) {
|
||
tick += delay_tick;
|
||
|
||
/* USER CODE BEGIN */
|
||
/* 定期向上位机发送包含姿态、云台、射击反馈的 MCU 状态包 */
|
||
AI_Send_MCU_Data();
|
||
/* USER CODE END */
|
||
|
||
osDelayUntil(tick);
|
||
}
|
||
} |