robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

自瞄接口

Browse Source
This commit is contained in:
xxxxm 2026-03-15 13:56:47 +08:00
parent 632cd81ddb
commit 66cc032a45
16 changed files with 679 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -93,7 +93,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/module/cmd/cmd_behavior.c User/module/cmd/cmd_behavior.c
User/module/cmd/cmd_example.c User/module/cmd/cmd_example.c
User/module/vision_bridge.c User/module/vision_bridge.c
User/module/aimbot.c
# User/task sources # User/task sources
User/task/ai.c User/task/ai.c
User/task/atti_esit.c User/task/atti_esit.c

1
User/bsp/uart.h
View File

@ -30,6 +30,7 @@ typedef enum {
BSP_UART_DR16, BSP_UART_DR16,
BSP_UART_VOFA, BSP_UART_VOFA,
BSP_UART_REF, BSP_UART_REF,
BSP_UART_AI,
BSP_UART_NUM, BSP_UART_NUM,
BSP_UART_ERR, BSP_UART_ERR,
} BSP_UART_t; } BSP_UART_t;

266
User/module/aimbot.c Normal file
View File

@ -0,0 +1,266 @@
#include "module/aimbot.h"
#include "device/device.h"
#include "bsp/uart.h"
#include "component/crc16.h"
#include <string.h>
/* =====================================================================
* FDCAN 帧索引（反馈方向：下层板?上层板）
* 注：上层板使用自己的IMU，下层板只发送射击数据与模式
* ===================================================================== */
#define FB_FRAME_DATA 0u /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B) */
/* =====================================================================
* UART 通信接口（上层板 <EFBFBD>?上位<EFBFBD>?PC<EFBFBD>?
* ===================================================================== */
int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai) {
if (BSP_UART_Receive(BSP_UART_AI, (uint8_t *)ai, sizeof(*ai), true) == DEVICE_OK) {
return DEVICE_OK;
}
return DEVICE_ERR;
}
int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result) {
if (ai->head[0] != 'M' || ai->head[1] != 'R') {
return DEVICE_ERR;
}
if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) {
return DEVICE_ERR;
}
result->mode = ai->mode;
result->gimbal_t.setpoint.yaw = ai->yaw;
result->gimbal_t.vel.yaw = ai->yaw_vel;
result->gimbal_t.accl.yaw = ai->yaw_acc;
result->gimbal_t.setpoint.pit = ai->pitch;
result->gimbal_t.vel.pit = ai->pitch_vel;
result->gimbal_t.accl.pit = ai->pitch_acc;
return DEVICE_OK;
}
/**
* @brief 打包 MCU 数据（UART 发给上位机格式），修正了原始实现中的字段错误<EFBFBD>?
* 四元数来<EFBFBD>?quat 参数；欧拉角、角速度来自 gimbal_fb->imu<EFBFBD>?
*/
int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb,
const AHRS_Quaternion_t *quat,
float bullet_speed, uint16_t bullet_count, uint8_t mode) {
if (mcu == NULL || gimbal_fb == NULL || quat == NULL) {
return DEVICE_ERR_NULL;
}
mcu->head[0] = 'M';
mcu->head[1] = 'R';
mcu->mode = mode;
mcu->q[0] = quat->q0;
mcu->q[1] = quat->q1;
mcu->q[2] = quat->q2;
mcu->q[3] = quat->q3;
mcu->yaw = gimbal_fb->imu.eulr.yaw;
mcu->yaw_vel = gimbal_fb->imu.gyro.z;
mcu->pitch = gimbal_fb->imu.eulr.pit;
mcu->pitch_vel = gimbal_fb->imu.gyro.x;
mcu->bullet_speed = bullet_speed;
mcu->bullet_count = bullet_count;
mcu->crc16 = CRC16_Calc((const uint8_t *)mcu,
sizeof(*mcu) - sizeof(uint16_t), CRC16_INIT);
if (!CRC16_Verify((const uint8_t *)mcu, sizeof(*mcu))) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu) {
if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)mcu, sizeof(*mcu), true) == DEVICE_OK) {
return DEVICE_OK;
}
return DEVICE_ERR;
}
/* =====================================================================
* CAN 通信接口（下层板 <EFBFBD>?上层板）
* ===================================================================== */
/**
* @brief 初始<EFBFBD>?Aimbot CAN 通信：注册指令接收队列和反馈收发队列<EFBFBD>?
* 下层板只需注册 cmd_id；上层板只需注册 fb_base_id <EFBFBD>?6 <EFBFBD>?ID<EFBFBD>?
* 本函数同时注册两侧所需 ID，上/下层板共用同一初始化流程即可<EFBFBD>?
*/
int8_t Aimbot_Init(Aimbot_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_FDCAN_Init();
/* 注册 AI 指令帧队列（下层板接收／上层板发送） */
BSP_FDCAN_RegisterId(param->can, param->cmd_id,
BSP_FDCAN_DEFAULT_QUEUE_SIZE);
/* 注册反馈数据帧队列（上层板接收／下层板发送） */
for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) {
BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i,
BSP_FDCAN_DEFAULT_QUEUE_SIZE);
}
return DEVICE_OK;
}
/**
* @brief <EFBFBD>?Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体<EFBFBD>?
/**
* @brief 【下层板】打包 CAN 反馈结构体。
*/
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
float bullet_speed, uint16_t bullet_count,
uint8_t mode) {
if (fb == NULL) {
return DEVICE_ERR_NULL;
}
fb->mode = mode;
fb->bullet_speed = bullet_speed;
fb->bullet_count = bullet_count;
return DEVICE_OK;
}
/**
* @brief 【下层板】将反馈数据打成 1 个 CAN 标准帧发给上层板。
*
* 帧格式（每帧 8 字节）：
* 帧0: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B)
*/
void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param,
const Aimbot_FeedbackData_t *fb) {
if (param == NULL || fb == NULL) return;
BSP_FDCAN_StdDataFrame_t frame;
frame.dlc = AIMBOT_CAN_DLC;
/* 帧0: 弹速 + 弹数 + 模式 */
frame.id = param->fb_base_id + FB_FRAME_DATA;
memcpy(&frame.data[0], &fb->bullet_speed, 4);
frame.data[4] = (uint8_t)(fb->bullet_count & 0xFFu);
frame.data[5] = (uint8_t)((fb->bullet_count >> 8u) & 0xFFu);
frame.data[6] = fb->mode;
frame.data[7] = 0u;
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
}
/**
* @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来<EFBFBD>?AI 指令并解析<EFBFBD>?
*
* 指令帧格式（8 字节，与 vision_bridge 一致）<EFBFBD>?
* data[0] : mode (1B)
* data[1..3.5] : yaw (28bit 有符号定点数<EFBFBD>?.1µrad/LSB)
* data[4.5..7] : pit (28bit 有符号定点数<EFBFBD>?.1µrad/LSB)
*
* @return DEVICE_OK 成功解析到新指令
* DEVICE_ERR 队列空，无新数据
*/
int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param,
Aimbot_AIResult_t *result) {
if (param == NULL || result == NULL) return DEVICE_ERR_NULL;
BSP_FDCAN_Message_t msg;
int8_t ret = DEVICE_ERR;
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg,
BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
result->mode = msg.data[0];
/* 解析 yaw（高 28 位），符号扩展为 int32 */
int32_t yaw_raw = (int32_t)(((uint32_t)msg.data[1] << 20u) |
((uint32_t)msg.data[2] << 12u) |
((uint32_t)msg.data[3] << 4u) |
((uint32_t)(msg.data[4] >> 4u) & 0xFu));
if (yaw_raw & 0x08000000) yaw_raw |= (int32_t)0xF0000000;
result->gimbal_t.setpoint.yaw = (float)yaw_raw / AIMBOT_ANGLE_SCALE;
/* 解析 pit（低 28 位），符号扩展为 int32 */
int32_t pit_raw = (int32_t)(((uint32_t)(msg.data[4] & 0xFu) << 24u) |
((uint32_t)msg.data[5] << 16u) |
((uint32_t)msg.data[6] << 8u) |
(uint32_t)msg.data[7]);
if (pit_raw & 0x08000000) pit_raw |= (int32_t)0xF0000000;
result->gimbal_t.setpoint.pit = (float)pit_raw / AIMBOT_ANGLE_SCALE;
ret = DEVICE_OK;
}
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 1, &msg,
BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
memcpy(&result->gimbal_t.vel.yaw, &msg.data[0], 4);
memcpy(&result->gimbal_t.vel.pit, &msg.data[4], 4);
ret = DEVICE_OK;
}
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 2, &msg,
BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
memcpy(&result->gimbal_t.accl.yaw, &msg.data[0], 4);
memcpy(&result->gimbal_t.accl.pit, &msg.data[4], 4);
ret = DEVICE_OK;
}
return ret;
}
/**
* @brief 【上层板】将 AI 指令通过 CAN 发送给下层板（单帧 8 字节）<EFBFBD>?
*
* <EFBFBD>?vision_bridge.c <EFBFBD>?AI_SendCmdOnFDCAN 编码格式完全一致，
* 上层板可直接调用本函数替<EFBFBD>?vision_bridge 中的同名函数<EFBFBD>?
*/
void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param,
const Aimbot_AIResult_t *cmd) {
if (param == NULL || cmd == NULL) return;
const int32_t yaw = (int32_t)(cmd->gimbal_t.setpoint.yaw * AIMBOT_ANGLE_SCALE);
const int32_t pit = (int32_t)(cmd->gimbal_t.setpoint.pit * AIMBOT_ANGLE_SCALE);
BSP_FDCAN_StdDataFrame_t frame = {0};
frame.id = param->cmd_id;
frame.dlc = AIMBOT_CAN_DLC;
frame.data[0] = cmd->mode;
frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF);
frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
frame.data[6] = (uint8_t)((pit >> 8) & 0xFF);
frame.data[7] = (uint8_t)(pit & 0xFF);
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
/* 第二帧：发速度 */
frame.id = param->cmd_id + 1;
memcpy(&frame.data[0], &cmd->gimbal_t.vel.yaw, 4);
memcpy(&frame.data[4], &cmd->gimbal_t.vel.pit, 4);
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
/* 第三帧：发加速度 */
frame.id = param->cmd_id + 2;
memcpy(&frame.data[0], &cmd->gimbal_t.accl.yaw, 4);
memcpy(&frame.data[4], &cmd->gimbal_t.accl.pit, 4);
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
}
/**
* @brief 【上层板】从 CAN 队列中非阻塞地解析下层板发来的反馈数据（共1帧）。
*
* @return DEVICE_OK
*/
int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param,
Aimbot_FeedbackData_t *fb) {
if (param == NULL || fb == NULL) return DEVICE_ERR_NULL;
BSP_FDCAN_Message_t msg;
/* 帧0: 弹速 + 弹数 + 模式 */
if (BSP_FDCAN_GetMessage(param->can,
param->fb_base_id + FB_FRAME_DATA,
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
memcpy(&fb->bullet_speed, &msg.data[0], 4);
fb->bullet_count = (uint16_t)(msg.data[4] | ((uint16_t)msg.data[5] << 8u));
fb->mode = msg.data[6];
}
return DEVICE_OK;
}

174
User/module/aimbot.h Normal file
View File

@ -0,0 +1,174 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component\user_math.h"
#include "module/gimbal.h"
#include "bsp/fdcan.h"
#include <stdint.h>
// 数据包协议定义（UART 通信使用 head[2]={'M', 'R'} 标识）
/* ============================ CAN 通信定义 ============================
* 通路：上位机 <--串口--> 上层板 <--CAN--> 下层板
*
* 注：上层板使用自己的IMU（atti_esti），下层板只需反馈电机和射击数据
*
* 下层板 → 上层板：反馈数据，单帧 8 字节
* 帧0 (fb_base_id+0): 弹速(4B) + 弹数(2B) + 模式(1B) + 保留(1B)
*
* 上层板 → 下层板：AI 指令，单帧 8 字节
* 帧 (cmd_id): mode(1B) + yaw(28bit) + pit(28bit)，定点数精度 0.1µrad
* ====================================================================== */
#define AIMBOT_FB_FRAME_NUM 1u /* 反馈数据帧总数（仅下层三字段） */
#define AIMBOT_CAN_DLC 8u /* CAN 帧数据长度 */
#define AIMBOT_ANGLE_SCALE 10000000.0f /* 指令角度定点数比例（0.1µrad/LSB） */
/* CAN 通信参数结构体 */
typedef struct {
BSP_FDCAN_t can; /* 使用的 CAN 总线实例 */
uint32_t cmd_id; /* 上层板→下层板 AI 指令帧 CAN ID */
uint32_t fb_base_id; /* 下层板→上层板 反馈数据起始 CAN ID */
} Aimbot_Param_t;
/* CAN 反馈数据结构体（下层板打包后经 CAN 发给上层板的内容）
* 注：下层板只反馈 bullet_speed / bullet_count / mode */
typedef struct {
uint8_t mode; /* 下层板当前工作模式 */
float bullet_speed; /* 弹速（m/s） */
uint16_t bullet_count; /* 子弹累计发射次数 */
} Aimbot_FeedbackData_t;
/*-----------------------------to上位机---------------------------------*/
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; // CRC16 校验
} Aimbot_MCU_t;
// 裁判系统数据结构
typedef struct __attribute__((packed))
{
uint16_t remain_hp; // 剩余血量
uint8_t game_progress : 4; // 比赛进度
uint16_t stage_remain_time; // 比赛剩余时间
} Aimbot_RefereeData_t;
typedef struct __attribute__((packed))
{
uint8_t id; // 数据包 ID: 0xA8
Aimbot_RefereeData_t data;
uint16_t crc16;
} Aimbot_Referee_t;
/*------------------------------------上位机back-------------------------------------*/
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; // CRC16 校验
} Aimbot_AI_t;
typedef struct __attribute__((packed)) {
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
struct{
// Gimbal_CMD_t g_cmd;
struct{
float yaw; // 目标偏航角
float pit; // 目标俯仰角
}setpoint;
struct{
float pit; // 俯仰角加速度
float yaw; // 偏航角加速度
}accl;
struct{
float pit; // 俯仰角速度
float yaw; // 偏航角速度
}vel;
}gimbal_t;
struct{
float Vx; // X 方向速度
float Vy; // Y 方向速度
float Vw; // Z 方向角速度
}chassis_t;
uint8_t reserved; // 预留字段
} Aimbot_AIResult_t; // 解析后的AI控制指令
/* ---------- UART 通信接口（上层板与上位机 PC 间） ---------- */
int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb,
const AHRS_Quaternion_t *quat,
float bullet_speed, uint16_t bullet_count, uint8_t mode);
int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu);
int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result);
int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai);
/* ---------- CAN 通信接口（下层板与上层板间） ---------- */
/**
* @brief 初始化 Aimbot CAN 通信，注册所有收发 CAN ID 队列
* @param param CAN 通信参数
* @return DEVICE_OK / DEVICE_ERR_NULL
*/
int8_t Aimbot_Init(Aimbot_Param_t *param);
/**
* @brief 【下层板】打包反馈结构体
* @param fb 输出：反馈数据
* @param bullet_speed 弹速（m/s）
* @param bullet_count 已发射子弹数
* @param mode 当前工作模式
*/
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
float bullet_speed, uint16_t bullet_count,
uint8_t mode);
/**
* @brief 【下层板】将反馈数据经 CAN 发送给上层板（1 帧）
*/
void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param,
const Aimbot_FeedbackData_t *fb);
/**
* @brief 【下层板】从 CAN 解析上层板发来的 AI 指令
* @return DEVICE_OK 表示成功取到新数据；DEVICE_ERR 表示队列暂无数据
*/
int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param,
Aimbot_AIResult_t *result);
/**
* @brief 【上层板】通过 CAN 将 AI 指令发送给下层板（单帧 8 字节）
*/
void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param,
const Aimbot_AIResult_t *cmd);
/**
* @brief 【上层板】从 CAN 解析下层板发来的反馈数据（逐帧读取，非阻塞）
* @return DEVICE_OK（部分或全部帧已更新均返回 OK）
*/
int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param,
Aimbot_FeedbackData_t *fb);
#ifdef __cplusplus
}
#endif

5
User/module/cmd/cmd.c
View File

@ -228,10 +228,7 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
/* 根据AI模式决定射击行为 */ /* 根据AI模式决定射击行为 */
switch (ctx->input.nuc.mode) { switch (ctx->input.nuc.mode) {
case 0: case 0:
ctx->output.shoot.cmd.ready = false; case 1:
ctx->output.shoot.cmd.firecmd = false;
break;
case 1:
ctx->output.shoot.cmd.ready = true; ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false; ctx->output.shoot.cmd.firecmd = false;
break; break;

6
User/module/cmd/cmd_feature.h
View File

@ -16,8 +16,8 @@
/** PC 端键鼠输入 (通过 DR16 转发) */ /** PC 端键鼠输入 (通过 DR16 转发) */
#define CMD_ENABLE_SRC_PC 1 #define CMD_ENABLE_SRC_PC 1
/** NUC / AI 输入 (需要 vision_bridge 模块) */ /** NUC / AI 输入 */
#define CMD_ENABLE_SRC_NUC 0 #define CMD_ENABLE_SRC_NUC 1
/** /**
* 裁判系统数据中转开关 * 裁判系统数据中转开关
@ -46,7 +46,7 @@
#define CMD_ENABLE_MODULE_ARM 0 #define CMD_ENABLE_MODULE_ARM 0
/** 裁判系统UI命令模块 (需要 device/referee.h) */ /** 裁判系统UI命令模块 (需要 device/referee.h) */
#define CMD_ENABLE_MODULE_REFUI 0 #define CMD_ENABLE_MODULE_REFUI 1
/** 平衡底盘模块 (需要 module/balance_chassis.h) */ /** 平衡底盘模块 (需要 module/balance_chassis.h) */
#define CMD_ENABLE_MODULE_BALANCE_CHASSIS 1 #define CMD_ENABLE_MODULE_BALANCE_CHASSIS 1

5
User/module/config.c
View File

@ -24,6 +24,11 @@ Config_RobotParam_t robot_config = {
.width=1920, .width=1920,
.height=1080, .height=1080,
}, },
.aimbot_param = {
.can = BSP_FDCAN_2,
.cmd_id = 0x520, /* 上层板→下层板 AI指令帧ID (共3帧: 0x520~0x522) */
.fb_base_id = 0x524, /* 下层板→上层板 反馈起始ID */
},
.gimbal_param = { .gimbal_param = {
.pid = { .pid = {
.yaw_omega = { .yaw_omega = {

2
User/module/config.h
View File

@ -16,6 +16,7 @@ extern "C" {
#include "module/vision_bridge.h" #include "module/vision_bridge.h"
#include "module/cmd/cmd.h" #include "module/cmd/cmd.h"
#include "device/referee_proto_types.h" #include "device/referee_proto_types.h"
#include "module/aimbot.h"
/** /**
* @brief 机器人参数配置结构体 * @brief 机器人参数配置结构体
* @note 在此添加您的配置参数 * @note 在此添加您的配置参数
@ -28,6 +29,7 @@ typedef struct {
AI_Param_t ai_param; AI_Param_t ai_param;
CMD_Config_t cmd_param; CMD_Config_t cmd_param;
Referee_Screen_t ref_screen; Referee_Screen_t ref_screen;
Aimbot_Param_t aimbot_param; /* 下层板 ↔ 上层板 CAN 通信参数 */
/* USER CODE END Config_RobotParam */ /* USER CODE END Config_RobotParam */
} Config_RobotParam_t; } Config_RobotParam_t;

65
User/module/gimbal.c
View File

@ -19,7 +19,8 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
#define GIMBAL_YAW_INERTIA 0.05f
#define GIMBAL_PIT_INERTIA 0.03f
/** /**
* \brief 设置云台模式 * \brief 设置云台模式
* *
@ -41,10 +42,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
PID_Reset(&g->pid.yaw_omega); PID_Reset(&g->pid.yaw_omega);
PID_Reset(&g->pid.pit_angle); PID_Reset(&g->pid.pit_angle);
PID_Reset(&g->pid.pit_omega); PID_Reset(&g->pid.pit_omega);
PID_Reset(&g->pid.aimbot.yaw_angle);
PID_Reset(&g->pid.aimbot.yaw_omega);
PID_Reset(&g->pid.aimbot.pit_angle);
PID_Reset(&g->pid.aimbot.pit_omega);
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
MOTOR_DM_Enable(&(g->param->yaw_motor)); MOTOR_DM_Enable(&(g->param->yaw_motor));
MOTOR_DM_Enable(&(g->param->pit_motor));
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
@ -91,6 +97,14 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
&(g->param->pid.pit_angle)); &(g->param->pid.pit_angle));
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq, PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.pit_omega)); &(g->param->pid.pit_omega));
PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.yaw_angle));
PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.yaw_omega));
PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.pit_angle));
PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.pit_omega));
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
g->param->low_pass_cutoff_freq.out); g->param->low_pass_cutoff_freq.out);
@ -193,33 +207,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
g->limit.pit.max, g->limit.pit.min, M_2PI); g->limit.pit.max, g->limit.pit.min, M_2PI);
} }
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) {
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
}
/* 控制相关逻辑 */ /* 控制相关逻辑 */
float yaw_omega_set_point, pit_omega_set_point; float yaw_omega_set_point, pit_omega_set_point;
switch (g->mode) { switch (g->mode) {
case GIMBAL_MODE_RELAX: case GIMBAL_MODE_RELAX:
g->out.yaw = 0.0f; g->out.yaw = 0.0f;
g->out.pit = 0.0f; g->out.pit = 0.0f;
break;
case GIMBAL_MODE_AI_CONTROL:
if (g_ai != NULL && g_ai->ctrl) {
g->setpoint.eulr.yaw = g_ai->yaw;
g->setpoint.eulr.pit = -g_ai->pit;
/* 限位处理 */
if (g->param->travel.yaw > 0) {
CircleAngleLimit(&g->setpoint.eulr.yaw,
g->feedback.motor.yaw.rotor_abs_angle,
g->feedback.imu.eulr.yaw,
g->limit.yaw.max, g->limit.yaw.min, M_2PI);
}
if (g->param->travel.pit > 0) {
CircleAngleLimit(&g->setpoint.eulr.pit,
g->feedback.motor.pit.rotor_abs_angle,
g->feedback.imu.eulr.rol,
g->limit.pit.max, g->limit.pit.min, M_2PI);
}
}
/* fallthrough - AI控制模式也需要执行PID计算 */ /* fallthrough - AI控制模式也需要执行PID计算 */
case GIMBAL_MODE_ABSOLUTE: case GIMBAL_MODE_ABSOLUTE:
@ -234,6 +233,28 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt); g->feedback.imu.gyro.y, 0.f, g->dt);
break;
break;
case GIMBAL_MODE_AI_CONTROL:
/* --- YAW --- */
// 位置环: Kp * (pos_ref - pos_fdb)
yaw_omega_set_point = PID_Calc(&(g->pid.aimbot.yaw_angle),
g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
// 速度环: Kd * (vel_ref - vel_fdb)，用上位机下发的 yaw_vel 作为前馈参考
g->out.yaw = PID_Calc(&(g->pid.aimbot.yaw_omega),
yaw_omega_set_point + g_cmd->ff_vel_yaw,
g->feedback.imu.gyro.z, 0.0f, g->dt)
+ g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc
/* --- PITCH --- */
pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle),
g->setpoint.eulr.pit,
g->feedback.imu.eulr.pit, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega),
pit_omega_set_point + g_cmd->ff_vel_pit,
g->feedback.imu.gyro.x, 0.0f, g->dt)
+ g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc
break; break;
} }

19
User/module/gimbal.h
View File

@ -42,6 +42,13 @@ typedef struct {
Gimbal_Mode_t mode; Gimbal_Mode_t mode;
float delta_yaw; float delta_yaw;
float delta_pit; float delta_pit;
/* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */
float setpoint_yaw;
float setpoint_pit;
float ff_vel_yaw; /* yaw 速度前馈（rad/s），叠加到内环角速度设定值 */
float ff_vel_pit; /* pit 速度前馈（rad/s） */
float ff_accl_yaw; /* yaw 加速度前馈（归一化），叠加到力矩输出 */
float ff_accl_pit; /* pit 加速度前馈（归一化） */
} Gimbal_CMD_t; } Gimbal_CMD_t;
typedef struct { typedef struct {
@ -66,6 +73,12 @@ typedef struct {
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */ KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
struct {
KPID_t yaw_angle; /* yaw轴角位置环PID */
KPID_t yaw_omega; /* yaw轴角速度环PID */
KPID_t pit_angle; /* pitch轴角位置环PID */
KPID_t pit_omega; /* pitch轴角速度环PID */
} aimbot;
} pid; } pid;
/* 低通滤波器截止频率 */ /* 低通滤波器截止频率 */
@ -128,6 +141,12 @@ typedef struct {
KPID_t yaw_omega; /* yaw轴角速度环PID */ KPID_t yaw_omega; /* yaw轴角速度环PID */
KPID_t pit_angle; /* pitch轴角位置环PID */ KPID_t pit_angle; /* pitch轴角位置环PID */
KPID_t pit_omega; /* pitch轴角速度环PID */ KPID_t pit_omega; /* pitch轴角速度环PID */
struct {
KPID_t yaw_angle; /* yaw轴角位置环PID */
KPID_t yaw_omega; /* yaw轴角速度环PID */
KPID_t pit_angle; /* pitch轴角位置环PID */
KPID_t pit_omega; /* pitch轴角速度环PID */
} aimbot;
} pid; } pid;
struct { struct {

203
User/module/shoot.c
View File

@ -41,15 +41,18 @@ void Task(void *argument) {
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define MAX_FRIC_RPM 7000.0f #define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 7000.0f//这里可能也会影响最高发射频率，待测试 #define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试
/* 发射检测参数 */ /* 发射检测参数 */
#define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */ #define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */
#define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */ #define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */
#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static bool last_firecmd; static bool last_firecmd;
float maxTrigrpm=4000.0f;
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/** /**
@ -180,7 +183,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
s->target_variable.fric_rpm=5000.0f; s->target_variable.fric_rpm=5000.0f;
break; break;
case SHOOT_PROJECTILE_42MM: case SHOOT_PROJECTILE_42MM:
s->target_variable.fric_rpm=6500.0f;//6500 s->target_variable.fric_rpm=5000.0f;//6500
break; break;
} }
return SHOOT_OK; return SHOOT_OK;
@ -207,7 +210,7 @@ static int8_t Shoot_UpdateHeatControl(Shoot_t *s)
s->heatcontrol.ncd = s->heatcontrol.Hcd / s->heatcontrol.Hgen; s->heatcontrol.ncd = s->heatcontrol.Hcd / s->heatcontrol.Hgen;
} else { } else {
s->heatcontrol.ncd = 0.0f; s->heatcontrol.ncd = 0.0f;
} }
return SHOOT_OK; return SHOOT_OK;
} }
@ -235,6 +238,40 @@ static float Shoot_CalcAvgFricRpm(Shoot_t *s)
return (fric_num > 0) ? (sum / fric_num) : 0.0f; return (fric_num > 0) ? (sum / fric_num) : 0.0f;
} }
/**
* \brief 发射检测：通过摩擦轮掉速检测发射
*
* \param s 包含发射数据的结构体
*
* \return 是否检测到掉速超过阈值
*/
static bool Shoot_DetectShotByRpmDrop(Shoot_t *s)
{
if (s == NULL) {
return false;
}
/* 只在READY和FIRE状态下检测，避免停机时误判 */
if (s->running_state != SHOOT_STATE_READY &&
s->running_state != SHOOT_STATE_FIRE) {
return false;
}
/* 计算当前摩擦轮平均转速 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
/* 计算当前转速与目标转速的差值（掉速量） */
s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm;
/* 判断是否发生明显掉速 */
if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) {
return true;
}
return false;
}
/** /**
* \brief 热量数据融合：用裁判系统数据修正自主估计 * \brief 热量数据融合：用裁判系统数据修正自主估计
* *
@ -318,29 +355,18 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
/* 计算当前平均转速 */ /* 计算当前平均转速 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
/* 检查摩擦轮是否初步达到设定速度 */ /* 检查摩擦轮是否达到目标转速的85%以上 */
if (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f) { if (s->heatcontrol.avgFricRpm >= s->target_variable.fric_rpm * FRIC_READY_RPM_RATIO) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; // 记录初始基准转速 s->heatcontrol.lastAvgFricRpm = s->heatcontrol.avgFricRpm;
} }
} }
break; break;
case SHOOT_HEAT_DETECT_READY: case SHOOT_HEAT_DETECT_READY:
/* 准备检测状态：监测摩擦轮相对当前的基准发生掉速 */ /* 准备检测状态：监测摩擦轮掉速 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); /* 检测掉速（当前转速低于目标转速超过阈值） */
if (Shoot_DetectShotByRpmDrop(s)) {
/* 动态更新平稳时的基准转速：若当前转速比基准高，迅速成为新基准；若更低，基准非常缓慢地衰减，以适应正常波动 */
if (s->heatcontrol.avgFricRpm > s->heatcontrol.baselineRpm) {
s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm;
} else {
s->heatcontrol.baselineRpm = s->heatcontrol.baselineRpm * 0.999f + s->heatcontrol.avgFricRpm * 0.001f;
}
s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm;
/* 检测掉速（当前转速低于动态基准超过阈值） */
if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED;
s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */ s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */
} }
@ -353,47 +379,45 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
case SHOOT_HEAT_DETECT_SUSPECTED: case SHOOT_HEAT_DETECT_SUSPECTED:
/* 发射嫌疑状态：持续检测掉速 */ /* 发射嫌疑状态：持续检测掉速 */
/* 更新掉速量 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm;
/* 若掉速消失（转速刚掉下又马上弹回基准附近），回到准备状态 (阈值的 60%) */ /* 若掉速消失（转速恢复正常），回到准备状态 */
if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.6f) { if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
} }
/* 若掉速状态持续超过设定的嫌疑时间，确认这是一次发射 */ /* 若嫌疑状态持续超过阈值时间，确认发射 */
else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) { else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED;
} }
break; break;
case SHOOT_HEAT_DETECT_CONFIRMED: case SHOOT_HEAT_DETECT_CONFIRMED:
if (s->heatcontrol.unverified_shots > 0) { /* 确认发射状态：增加热量并返回准备状态 */
s->heatcontrol.unverified_shots--; /* 根据弹丸类型增加估计热量 */
} switch (s->param->basic.projectileType) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_RECOVERING; case SHOOT_PROJECTILE_17MM:
s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; /* 记录进入此时的转速为谷底初值 */ s->heatcontrol.Hnow_estimated += 10.0f;
break; break;
case SHOOT_PROJECTILE_42MM:
case SHOOT_HEAT_DETECT_RECOVERING: s->heatcontrol.Hnow_estimated += 100.0f;
break;
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); default:
s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen;
if (s->heatcontrol.avgFricRpm < s->heatcontrol.valleyRpm) { break;
s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm;
} }
if ((s->heatcontrol.avgFricRpm - s->heatcontrol.valleyRpm > 30.0f) || /* 限制估计热量不超过最大值 */
(s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f)) { if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) {
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm;
} }
/* 如果摩擦轮停止 */ /* 增加发射计数 */
if (s->running_state == SHOOT_STATE_IDLE) { s->heatcontrol.shots_detected++;
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE;
} /* 返回准备检测状态 */
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
break; break;
default: default:
@ -401,7 +425,7 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
break; break;
} }
/* 热量冷却*/ /* 善后工作：热量冷却（每个周期都执行） */
if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) { if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) {
s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt; s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt;
if (s->heatcontrol.Hnow_estimated < 0.0f) { if (s->heatcontrol.Hnow_estimated < 0.0f) {
@ -409,36 +433,6 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
} }
} }
/* 空弹链检测与修正：拨盘触发后超过0.4秒内仍未有对应的摩擦轮掉速确认 */
if (s->heatcontrol.unverified_shots > 0 && (s->timer.now - s->var_trig.time_lastShoot) > 0.4f) {
/* 这些弹丸确定未射出（空发） */
float refund_heat = 0.0f;
switch (s->param->basic.projectileType) {
case SHOOT_PROJECTILE_17MM: refund_heat = 10.0f; break;
case SHOOT_PROJECTILE_42MM: refund_heat = 100.0f; break;
default: refund_heat = s->heatcontrol.Hgen; break;
}
refund_heat *= s->heatcontrol.unverified_shots;
s->heatcontrol.Hnow_estimated -= refund_heat;
if (s->heatcontrol.Hnow_estimated < 0.0f) {
s->heatcontrol.Hnow_estimated = 0.0f;
}
if (s->var_trig.num_shooted >= s->heatcontrol.unverified_shots) {
s->var_trig.num_shooted -= s->heatcontrol.unverified_shots;
} else {
s->var_trig.num_shooted = 0;
}
if (s->heatcontrol.shots_detected >= s->heatcontrol.unverified_shots) {
s->heatcontrol.shots_detected -= s->heatcontrol.unverified_shots;
} else {
s->heatcontrol.shots_detected = 0;
}
s->heatcontrol.unverified_shots = 0;
}
/* 数据融合 */ /* 数据融合 */
Shoot_FuseHeatData(s); Shoot_FuseHeatData(s);
@ -508,22 +502,18 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s)
*/ */
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{ {
if (s == NULL) { if (s == NULL || s->var_trig.num_toShoot == 0) {
return SHOOT_ERR_NULL; return SHOOT_ERR_NULL;
} }
/* 更新热量控制数据 */ /* 更新热量控制数据 */
Shoot_UpdateHeatControl(s); Shoot_UpdateHeatControl(s);
if (s->var_trig.num_toShoot == 0) {
return SHOOT_OK; /* 即使没有弹丸待发，热量更新照常执行 */
}
/* 根据热量控制计算实际射频 */ /* 根据热量控制计算实际射频 */
float actual_freq = Shoot_CaluFreqByHeat(s); float actual_freq = Shoot_CaluFreqByHeat(s);
float dt = s->timer.now - s->var_trig.time_lastShoot; float dt = s->timer.now - s->var_trig.time_lastShoot;
float dpos; float dpos;
dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI);
/* 使用热量控制计算出的射频，而不是配置的固定射频 */ /* 使用热量控制计算出的射频，而不是配置的固定射频 */
@ -531,25 +521,10 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{ {
s->var_trig.time_lastShoot=s->timer.now; s->var_trig.time_lastShoot=s->timer.now;
CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI);
s->var_trig.num_toShoot--; s->var_trig.num_toShoot--;
s->var_trig.num_shooted++; s->var_trig.num_shooted++;
/* 预测性发射：拨盘拨动即认为弹丸会射出，立即增加热量与发射计数 */
float add_heat = 0.0f;
switch (s->param->basic.projectileType) {
case SHOOT_PROJECTILE_17MM: add_heat = 10.0f; break;
case SHOOT_PROJECTILE_42MM: add_heat = 100.0f; break;
default: add_heat = s->heatcontrol.Hgen; break;
}
s->heatcontrol.Hnow_estimated += add_heat;
if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) {
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
}
s->heatcontrol.shots_detected++;
/* 增加等待摩擦轮掉速确认的弹丸数 */
s->heatcontrol.unverified_shots++;
} }
return SHOOT_OK; return SHOOT_OK;
} }
@ -630,7 +605,7 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s)
s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl; s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl;
} }
s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed); s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed);
s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / MAX_TRIG_RPM; s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm;
if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f; if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f;
if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f; if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f;
@ -652,17 +627,16 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
return SHOOT_ERR_NULL; // 参数错误 return SHOOT_ERR_NULL; // 参数错误
} }
uint8_t fric_num = s->param->basic.fric_num; uint8_t fric_num = s->param->basic.fric_num;
static float pos;
if(s->mode==SHOOT_MODE_SAFE){ if(s->mode==SHOOT_MODE_SAFE){
for(int i=0;i<fric_num;i++) for(int i=0;i<fric_num;i++)
{ {
MOTOR_RM_Relax(&s->param->motor.fric[i].param); MOTOR_RM_Relax(&s->param->motor.fric[i].param);
} }
MOTOR_RM_Relax(&s->param->motor.trig); MOTOR_RM_Relax(&s->param->motor.trig);\
s->target_variable.trig_angle=s->var_trig.trig_agl; pos=s->target_variable.trig_angle=s->var_trig.trig_agl;
} }
else{ else{
Shoot_CaluTargetAngle(s, cmd);
switch(s->running_state) switch(s->running_state)
{ {
case SHOOT_STATE_IDLE:/*熄火等待*/ case SHOOT_STATE_IDLE:/*熄火等待*/
@ -675,7 +649,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]); MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
} }
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.trig_angle,s->var_trig.trig_agl,0,s->timer.dt); s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig); MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
@ -719,7 +693,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
} }
/* 设置拨弹电机输出 */ /* 设置拨弹电机输出 */
s->output.outagl_trig =PID_Calc(&s->pid.trig, s->output.outagl_trig =PID_Calc(&s->pid.trig,
s->target_variable.trig_angle, pos,
s->var_trig.trig_agl, s->var_trig.trig_agl,
0, 0,
s->timer.dt); s->timer.dt);
@ -761,6 +735,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
break; break;
case SHOOT_STATE_FIRE:/*射击*/ case SHOOT_STATE_FIRE:/*射击*/
Shoot_CaluTargetAngle(s, cmd);
for(int i=0;i<fric_num;i++) for(int i=0;i<fric_num;i++)
{ {
uint8_t level=s->param->motor.fric[i].level-1; uint8_t level=s->param->motor.fric[i].level-1;
@ -805,7 +780,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
if(!cmd->firecmd) if(!cmd->firecmd)
{ {
s->running_state=SHOOT_STATE_READY; s->running_state=SHOOT_STATE_READY;
s->target_variable.trig_angle=s->var_trig.trig_agl; pos=s->var_trig.trig_agl;
s->var_trig.num_toShoot=0; s->var_trig.num_toShoot=0;
} }
break; break;
@ -991,10 +966,10 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
s->timer.now = BSP_TIME_Get_us() / 1000000.0f; s->timer.now = BSP_TIME_Get_us() / 1000000.0f;
s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f; s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f;
s->timer.lask_wakeup = BSP_TIME_Get_us(); s->timer.lask_wakeup = BSP_TIME_Get_us();
Shoot_CaluTargetRPM(s,233); // Shoot_CaluTargetRPM(s,233);
/* 运行热量检测状态机 */ /* 运行热量检测状态机 */
Shoot_HeatDetectionFSM(s); // Shoot_HeatDetectionFSM(s);
/* 运行卡弹检测状态机 */ /* 运行卡弹检测状态机 */
Shoot_JamDetectionFSM(s, cmd); Shoot_JamDetectionFSM(s, cmd);
@ -1013,3 +988,7 @@ void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui) {
ui->fire = s->running_state; ui->fire = s->running_state;
} }
void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) {
ai->num_shooted=s->var_trig.num_shooted;
ai->bullet_speed=12.0f;
}

15
User/module/shoot.h
View File

@ -35,8 +35,7 @@ typedef enum {
SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */ SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */
SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */ SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */
SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */ SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */
SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */ SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */
SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */
}Shoot_HeatDetectionFSM_State_t; }Shoot_HeatDetectionFSM_State_t;
typedef enum { typedef enum {
SHOOT_STATE_IDLE = 0,/* 熄火 */ SHOOT_STATE_IDLE = 0,/* 熄火 */
@ -52,6 +51,11 @@ typedef enum {
SHOOT_MODE_NUM SHOOT_MODE_NUM
}Shoot_Mode_t; }Shoot_Mode_t;
typedef struct {
uint16_t num_shooted;/* 已发射弹数 */
float bullet_speed;/* 目标弹速 */
}Shoot_AI_t;
/* UI 导出结构（供 referee 系统绘制） */ /* UI 导出结构（供 referee 系统绘制） */
typedef struct { typedef struct {
Shoot_Mode_t mode; Shoot_Mode_t mode;
@ -115,8 +119,7 @@ typedef struct {
Shoot_HeatDetectionFSM_State_t detectState; // 检测状态 Shoot_HeatDetectionFSM_State_t detectState; // 检测状态
float detectTime; // 检测计时器 float detectTime; // 检测计时器
float avgFricRpm; // 摩擦轮平均转速 float avgFricRpm; // 摩擦轮平均转速
float baselineRpm; // 动态基准转速，用于连发掉速检测的参照 float lastAvgFricRpm; // 上次摩擦轮平均转速
float valleyRpm; // 掉速谷底转速，用于判断掉速回升
float rpmDrop; // 转速下降量 float rpmDrop; // 转速下降量
bool shotDetected; // 检测到发射标志 bool shotDetected; // 检测到发射标志
@ -125,7 +128,6 @@ typedef struct {
float Hnow_fused; // 融合后的热量值 float Hnow_fused; // 融合后的热量值
uint16_t shots_detected; // 检测到的发射数 uint16_t shots_detected; // 检测到的发射数
uint16_t shots_available;// 当前热量可发射弹数 uint16_t shots_available;// 当前热量可发射弹数
uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数
}Shoot_HeatControl_t; }Shoot_HeatControl_t;
typedef struct { typedef struct {
@ -219,6 +221,7 @@ typedef struct {
struct { struct {
float fric_rpm; /* 目标摩擦轮转速 */ float fric_rpm; /* 目标摩擦轮转速 */
float trig_angle;/* 目标拨弹位置 */ float trig_angle;/* 目标拨弹位置 */
float bullet_speed;/* 目标弹速 */
}target_variable; }target_variable;
/* 反馈控制用的PID */ /* 反馈控制用的PID */
@ -295,7 +298,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
*/ */
void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui); void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui);
void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) ;
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

73
User/task/ai.c
View File

@ -1,15 +1,16 @@
/* /*
ai Task AI Task - 下层板
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/fdcan.h" #include "module/aimbot.h"
#include "module/config.h" #include "module/config.h"
#include "module/gimbal.h" #include "module/gimbal.h"
#include "module/vision_bridge.h" #include "module/shoot.h"
#include "device/referee_proto_types.h"
#include <string.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,20 +18,20 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
AI_cmd_t cmd_ai; static Aimbot_Param_t aimbot_can; /* CAN 通信参数（从 config 拷贝） */
AI_t ai; static Aimbot_FeedbackData_t lower_board_fb; /* 打包好发给上层板的数据 */
Gimbal_AI_t ai_for_gimbal; static Aimbot_AIResult_t ai_cmd_from_can; /* 上层板下发并传到底板的指令 */
static AI_cmd_t for_cmdnuc; /* 转换后发往 cmd.nuc 的指令 */
static Referee_ForAI_t ai_ref; /* 裁判系统转发给 AI 的数据 */
static Shoot_AI_t raw_shoot; /* 发射机构反馈 */
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
/* USER PRIVATE CODE BEGIN */
/* USER PRIVATE CODE END */
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
void Task_ai(void *argument) { void Task_ai(void *argument) {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */ /* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ; const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
@ -38,25 +39,49 @@ void Task_ai(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
AI_Init(&ai, &Config_GetRobotParam()->ai_param); aimbot_can = Config_GetRobotParam()->aimbot_param;
/* 注册CAN接收ID */ Aimbot_Init(&aimbot_can);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
AI_ParseCmdFromCan( &ai,&cmd_ai);
if (cmd_ai.mode != 0){ /* 1. 读取裁判系统数据（获知机器人系统状态/弹速等） */
ai_for_gimbal.ctrl = true; osMessageQueueGet(task_runtime.msgq.ai.ref, &ai_ref, NULL, 0);
} else {
ai_for_gimbal.ctrl = false; /* 2. 读取发射机构反馈数据（非阻塞），以获取弹速、弹数等信息
todo: 检查是否有单独发弹速消息队列
*/
osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromShoot, &raw_shoot, NULL, 0);
/* 3. 打包当前底层板的数据（弹速、弹数、模式），通过CAN发送给上层板
由于现在只能拿到一部分，预留测试值，实车时接入射击模块返回的信息
*/
// TODO: 使用实际读取到的 bullet_speed 和 bullet_count
float current_bullet_speed = raw_shoot.bullet_speed;
uint16_t current_bullet_count = raw_shoot.num_shooted;
uint8_t current_mode = 1;
Aimbot_PackFeedback(&lower_board_fb, current_bullet_speed, current_bullet_count, current_mode);
Aimbot_SendFeedbackOnCAN(&aimbot_can, &lower_board_fb);
/* 4. 解析上层板发来的 AI 指令（非阻塞提取） */
if (Aimbot_ParseCmdFromCAN(&aimbot_can, &ai_cmd_from_can) == DEVICE_OK) {
/* 将 Aimbot_AIResult_t 转换为 AI_cmd_t 并推送到 cmd.nuc 队列 */
for_cmdnuc.mode = ai_cmd_from_can.mode;
for_cmdnuc.gimbal.setpoint.yaw = ai_cmd_from_can.gimbal_t.setpoint.yaw;
for_cmdnuc.gimbal.setpoint.pit = ai_cmd_from_can.gimbal_t.setpoint.pit;
for_cmdnuc.gimbal.vel.yaw = ai_cmd_from_can.gimbal_t.vel.yaw;
for_cmdnuc.gimbal.vel.pit = ai_cmd_from_can.gimbal_t.vel.pit;
for_cmdnuc.gimbal.accl.yaw = ai_cmd_from_can.gimbal_t.accl.yaw;
for_cmdnuc.gimbal.accl.pit = ai_cmd_from_can.gimbal_t.accl.pit;
osMessageQueueReset(task_runtime.msgq.cmd.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.nuc, &for_cmdnuc, 0, 0);
} }
ai_for_gimbal.yaw = cmd_ai.gimbal.setpoint.yaw;
ai_for_gimbal.pit = cmd_ai.gimbal.setpoint.pit;
osMessageQueueReset(task_runtime.msgq.gimbal.ai_cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.ai_cmd, &ai_for_gimbal, 0, 0);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }
} }

11
User/task/cmd.c
View File

@ -13,7 +13,7 @@
#include "module/shoot.h" #include "module/shoot.h"
#include "module/cmd/cmd.h" #include "module/cmd/cmd.h"
#include "bsp/fdcan.h" #include "bsp/fdcan.h"
#include "module/vision_bridge.h"
//#define DEAD_AREA 0.05f //#define DEAD_AREA 0.05f
/* USER INCLUDE END */ /* USER INCLUDE END */
@ -28,7 +28,7 @@ DR16_t cmd_dr16;
#elif CMD_RCTypeTable_Index == 1 #elif CMD_RCTypeTable_Index == 1
AT9S_t cmd_at9s; AT9S_t cmd_at9s;
#endif #endif
// AI_cmd_t cmd_ai; AI_cmd_t cmd_ai;
#if CMD_ENABLE_SRC_REF #if CMD_ENABLE_SRC_REF
CMD_RawInput_REF_t cmd_ref; CMD_RawInput_REF_t cmd_ref;
@ -53,8 +53,6 @@ void Task_cmd(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param);
// AI_Init(&ai, &Config_GetRobotParam()->ai_param);
/* 注册CAN接收ID */
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
@ -65,10 +63,7 @@ void Task_cmd(void *argument) {
#elif CMD_RCTypeTable_Index == 1 #elif CMD_RCTypeTable_Index == 1
osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0);
#endif #endif
osMessageQueueGet(task_runtime.msgq.cmd.nuc, &cmd_ai, NULL, 0);
// /* 从CAN2接收AI命令 */
// AI_ParseCmdFromCan( &ai,&cmd_ai);
// #error "弄好自瞄之后统一改"
#if CMD_ENABLE_SRC_REF #if CMD_ENABLE_SRC_REF
/* 从裁判系统读取最新数据（非阻塞） */ /* 从裁判系统读取最新数据（非阻塞） */

7
User/task/init.c
View File

@ -61,7 +61,12 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL); task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL); task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
/* 裁判系统 */
/* AI */
task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL);
task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
task_runtime.msgq.ai.rawdata_FromShoot = osMessageQueueNew(2u, sizeof(Shoot_AI_t), NULL);
/* 裁判系统 */
task_runtime.msgq.referee.tocmd.ai= osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL); task_runtime.msgq.referee.tocmd.ai= osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
task_runtime.msgq.referee.tocmd.cap= osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL); task_runtime.msgq.referee.tocmd.cap= osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
task_runtime.msgq.referee.tocmd.chassis= osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL); task_runtime.msgq.referee.tocmd.chassis= osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);

13
User/task/user_task.h
View File

@ -87,14 +87,13 @@ typedef struct {
struct { struct {
osMessageQueueId_t rc; osMessageQueueId_t rc;
osMessageQueueId_t ref; osMessageQueueId_t ref;
osMessageQueueId_t ai; osMessageQueueId_t nuc;
}cmd; }cmd;
struct { struct{
osMessageQueueId_t quat; osMessageQueueId_t rawdata_FromGimbal;
osMessageQueueId_t move_vec; osMessageQueueId_t rawdata_FromShoot;
osMessageQueueId_t eulr; osMessageQueueId_t rawdata_FromIMU;
osMessageQueueId_t fire; osMessageQueueId_t ref; /* Referee_ForAI_t, cmd转发 */
osMessageQueueId_t ref;
}ai; }ai;
struct{ struct{
osMessageQueueId_t ref; /* Referee_ForCap_t, cmd转发 */ osMessageQueueId_t ref; /* Referee_ForCap_t, cmd转发 */