zzzhkgs/Drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

modified: MDK-ARM/DevC.uvoptx

Browse Source 
modified:   MDK-ARM/DevC.uvprojx
	modified:   MDK-ARM/DevC/DevC.axf
	modified:   MDK-ARM/DevC/DevC.hex
	modified:   User/bsp/uart.c
	modified:   User/bsp/uart.h
	modified:   User/module/config.c
	modified:   User/module/gimbal.c
	modified:   User/module/gimbal.h
	modified:   User/module/shoot.c
	modified:   User/task/ai.c
	modified:   User/task/atti_esti.c
	modified:   User/task/ctrl_gimbal.c
	modified:   User/task/rc.c
This commit is contained in:
zzzhkgs@gmail.com 2026-03-16 03:47:37 +08:00
parent cf8c530dc8
commit c1a0c821c3
14 changed files with 8551 additions and 6150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
MDK-ARM/DevC.uvoptx
View File

@ -188,12 +188,12 @@
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>\\DevC\../User/module/config.c\robot_config.gimbal_param.mech_zero.pit</ItemText> <ItemText>tx_pkg</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>7</count> <count>7</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>BSP_UART_AI</ItemText> <ItemText>tx_pkg</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>8</count> <count>8</count>
@ -203,14 +203,81 @@
<Ww> <Ww>
<count>9</count> <count>9</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>tx_pkg</ItemText> <ItemText>task_runtime</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>10</count> <count>10</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>tx_pkg</ItemText> <ItemText>gimbal_imu_to_send</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal_ahrs</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>ai_rx_data</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>ai_rx_buf</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>ai_rx_buf</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>vision_data</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>vision_data1</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal_cmd</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>last_vision_data</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>last_vision_update_tick</ItemText>
</Ww>
<Ww>
<count>20</count>
<WinNumber>1</WinNumber>
<ItemText>last_vision_update_tick</ItemText>
</Ww>
<Ww>
<count>21</count>
<WinNumber>1</WinNumber>
<ItemText>fan</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<WatchWindow2>
<Ww>
<count>0</count>
<WinNumber>2</WinNumber>
<ItemText>vision_data1</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>2</WinNumber>
<ItemText>gimbal</ItemText>
</Ww>
</WatchWindow2>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
</Tracepoint> </Tracepoint>

2
MDK-ARM/DevC.uvprojx
View File

@ -315,7 +315,7 @@
</ArmAdsMisc> </ArmAdsMisc>
<Cads> <Cads>
<interw>1</interw> <interw>1</interw>
<Optim>4</Optim> <Optim>1</Optim>
<oTime>0</oTime> <oTime>0</oTime>
<SplitLS>0</SplitLS> <SplitLS>0</SplitLS>
<OneElfS>1</OneElfS> <OneElfS>1</OneElfS>

BIN
MDK-ARM/DevC/DevC.axf
View File

Binary file not shown.

13844
MDK-ARM/DevC/DevC.hex
View File

File diff suppressed because it is too large Load Diff

29
User/bsp/uart.c
View File

@ -160,4 +160,33 @@ int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma)
/* USER FUNCTION BEGIN */ /* USER FUNCTION BEGIN */
/**
* @brief 开启串口空闲中断 + DMA 接收
* @note 调用 HAL 库原生的接收函数，配合自定义的 IDLE 中断处理使用
*/
int8_t BSP_UART_Receive_IDLE(BSP_UART_t uart, uint8_t *data, uint16_t size) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
/* 使用标准 DMA 接收，IDLE 标志将在你的 BSP_UART_IRQHandler 中被处理 */
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size) != HAL_OK) {
return BSP_ERR;
}
return BSP_OK;
}
/**
* @brief 获取 DMA 当前实际接收到的数据长度
* @param max_size 开启 DMA 接收时传入的最大缓冲区长度
*/
uint16_t BSP_UART_GetRxCount(BSP_UART_t uart, uint16_t max_size) {
UART_HandleTypeDef *huart = BSP_UART_GetHandle(uart);
if (huart == NULL || huart->hdmarx == NULL) return 0;
/* 实际接收长度 = 缓冲区总容量 - DMA通道剩余未传输数据量 */
return max_size - __HAL_DMA_GET_COUNTER(huart->hdmarx);
}
/* USER FUNCTION END */ /* USER FUNCTION END */

3
User/bsp/uart.h
View File

@ -62,7 +62,8 @@ int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
/* USER FUNCTION BEGIN */ /* USER FUNCTION BEGIN */
int8_t BSP_UART_Receive_IDLE(BSP_UART_t uart, uint8_t *data, uint16_t size);
uint16_t BSP_UART_GetRxCount(BSP_UART_t uart, uint16_t max_size);
/* USER FUNCTION END */ /* USER FUNCTION END */
#ifdef __cplusplus #ifdef __cplusplus

20
User/module/config.c
View File

@ -36,7 +36,7 @@ Config_RobotParam_t robot_config = {
}, },
.yaw_angle = { .yaw_angle = {
.k = 7.0f, .k = 7.0f,
.p = 3.5f, .p = 5.5f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
@ -56,21 +56,21 @@ Config_RobotParam_t robot_config = {
}, },
.pit_angle = { .pit_angle = {
.k = 2.5f, .k = 2.5f,
.p = 5.0f, .p = 1.0f,
.i = 0.2f, .i = 0.5f,
.d = 0.01f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.5f,
.out_limit = 10.0f, .out_limit = 5.2f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = M_2PI, .range = M_2PI,
}, },
}, },
.mech_zero = { .mech_zero = {
.yaw = 2.9f, .yaw = 3.0f,
.pit = 1.39f, .pit = 1.39f,
}, },
.travel = { .travel = {
.yaw = 1.0f, .yaw = 0.8f,
.pit = 1.2f, .pit = 1.2f,
}, },
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
@ -104,7 +104,7 @@ Config_RobotParam_t robot_config = {
.shoot_param = { .shoot_param = {
.trig_step_angle=M_2PI/7.8, .trig_step_angle=M_2PI/8,
.shot_delay_time=0.08f, .shot_delay_time=0.08f,
.shot_burst_num=1, .shot_burst_num=1,
.fric_motor_param[0] = { .fric_motor_param[0] = {
@ -134,7 +134,7 @@ Config_RobotParam_t robot_config = {
.i=0.0f, .i=0.0f,
.d=0.0f, .d=0.0f,
.i_limit=0.0f, .i_limit=0.0f,
.out_limit=0.9f, .out_limit=0.3f,
.d_cutoff_freq=30.0f, .d_cutoff_freq=30.0f,
.range=-1.0f, .range=-1.0f,
}, },

210
User/module/gimbal.c
View File

@ -18,8 +18,11 @@
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define YAW_MAX_OUTPUT 0.3f /* Yaw轴(大疆电机)最大输出比例限制，范围：0.0 到 1.0 */ #define YAW_MAX_OUTPUT 0.8f /* Yaw轴(大疆电机)最大输出比例限制，范围：0.0 到 1.0 */
#define PIT_MAX_TORQUE 1.0f /* Pitch轴(达妙电机)最大扭矩限制，单位：N·m (请根据实际情况调整) */ #define PIT_MAX_TORQUE 2.0f /* Pitch轴(达妙电机)最大扭矩限制，单位：N·m (请根据实际情况调整) */
static LowPassFilter2p_t vision_yaw_lpf;
static LowPassFilter2p_t vision_pit_lpf;
static uint8_t is_vision_lpf_init = 0;
/** /**
* \brief 设置云台模式 * \brief 设置云台模式
* *
@ -136,12 +139,12 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
gimbal->feedback.imu.eulr = imu->eulr; gimbal->feedback.imu.eulr = imu->eulr;
return 0; return 0;
} }
int fan = 0;
/** /**
* \brief 运行云台控制逻辑 * \brief 运行云台控制逻辑（集成 AI 速度前馈）
* *
* \param g 包含云台数据的结构体 * \param g 包含云台数据的结构体
* \param g_cmd 云台控制指令 * \param g_cmd 云台控制指令（包含遥控器增量及视觉数据）
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
@ -150,98 +153,96 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
return -1; return -1;
} }
/* 更新时间步长 */
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f; g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
g->lask_wakeup = BSP_TIME_Get_us(); g->lask_wakeup = BSP_TIME_Get_us();
/* 模式切换处理 */
Gimbal_SetMode(g, g_cmd->mode); Gimbal_SetMode(g, g_cmd->mode);
/* --- 计算目标增量：兼容手动模式与火控托管模式 --- */ /* --- 1. 计算目标设定值增量 --- */
float delta_yaw = 0.0f; float delta_yaw = 0.0f;
float delta_pit = 0.0f; float delta_pit = 0.0f;
if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) { if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) {
/* 火控模式且目标已锁定：计算视觉目标角度与当前设定值的偏差 */ /* 火控模式且目标已锁定：计算视觉目标角度与当前设定值的偏差 */
delta_yaw = CircleError(g_cmd->vision.yaw, g->setpoint.eulr.yaw, M_2PI); delta_yaw = CircleError((g_cmd->vision.yaw), g->setpoint.eulr.yaw, M_2PI);
delta_pit = CircleError(g_cmd->vision.pit, g->setpoint.eulr.pit, M_2PI); if (fan) delta_pit = CircleError((g_cmd->vision.pit), g->setpoint.eulr.pit, M_2PI);
else delta_pit = CircleError((g_cmd->vision.pit), g->setpoint.eulr.pit, M_2PI);
// delta_pit = CircleError(g_cmd->vision.pit, g->setpoint.eulr.pit, M_2PI);
} else { } else {
/* 常规模式或火控模式下目标丢失：使用操作手遥控器增量 */ /* 常规模式或火控模式下目标丢失：使用操作手遥控器增量 */
delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f; delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
delta_pit = g_cmd->delta_pit * g->dt; delta_pit = g_cmd->delta_pit * g->dt;
} }
/* --- 处理yaw控制命令，软件限位 - 使用电机绝对角度 --- */ /* --- 2. 软件限位处理 (Yaw) --- */
if (g->param->travel.yaw > 0) { if (g->param->travel.yaw > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw; float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI; if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI; if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 */ const float delta_max = CircleError(g->limit.yaw.max, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
const float delta_max = CircleError(g->limit.yaw.max, const float delta_min = CircleError(g->limit.yaw.min, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
const float delta_min = CircleError(g->limit.yaw.min,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
/* 限制控制命令 */
if (delta_yaw > delta_max) delta_yaw = delta_max; if (delta_yaw > delta_max) delta_yaw = delta_max;
if (delta_yaw < delta_min) delta_yaw = delta_min; if (delta_yaw < delta_min) delta_yaw = delta_min;
} else {
/* 无限位时直接使用电机绝对角度进行限位 */
float current_motor_angle = g->feedback.motor.yaw.rotor_abs_angle;
float target_angle = g->setpoint.eulr.yaw + delta_yaw;
/* 归一化角度到[-π, π] */
while (target_angle > M_PI) target_angle -= M_2PI;
while (target_angle < -M_PI) target_angle += M_2PI;
while (current_motor_angle > M_PI) current_motor_angle -= M_2PI;
while (current_motor_angle < -M_PI) current_motor_angle += M_2PI;
} }
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI); CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
/* --- 处理pitch控制命令，软件限位 - 使用电机绝对角度 --- */ /* --- 3. 软件限位处理 (Pitch) --- */
if (g->param->travel.pit > 0) { if (g->param->travel.pit > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol; float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI; if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI; if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 */ const float delta_max = CircleError(g->limit.pit.max, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
const float delta_max = CircleError(g->limit.pit.max, const float delta_min = CircleError(g->limit.pit.min, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
const float delta_min = CircleError(g->limit.pit.min,
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
/* 限制控制命令 */
if (delta_pit > delta_max) delta_pit = delta_max; if (delta_pit > delta_max) delta_pit = delta_max;
if (delta_pit < delta_min) delta_pit = delta_min; if (delta_pit < delta_min) delta_pit = delta_min;
} }
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
/* --- 控制相关逻辑（PID闭环计算） --- */ /* --- 4. 闭环控制计算 (串级 PID + 前馈) --- */
float yaw_omega_set_point, pit_omega_set_point; float yaw_omega_set_point, pit_omega_set_point;
float yaw_v_ff = 0.0f, pit_v_ff = 0.0f;
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; break;
case GIMBAL_MODE_AUTO_AIM: /* 火控模式与绝对模式共用相同的空间系PID闭环 */ case GIMBAL_MODE_AUTO_AIM:
/* 如果视觉在线，提取前馈项 */
if (g_cmd->vision.target_found) {
g->setpoint.eulr.pit=g_cmd->vision.pit;
/* 前馈系数建议放在参数结构体中，此处使用 1.0f 作为默认增益 */
yaw_v_ff = g_cmd->vision.yaw_v_ff * 0.2f;
pit_v_ff = g_cmd->vision.pit_v_ff * 0.2f;
}
/* 顺延执行 PID 计算 */
case GIMBAL_MODE_ABSOLUTE: case GIMBAL_MODE_ABSOLUTE:
/* Yaw 轴：角度环 -> 速度环 + 前馈 */
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt); g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point, g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt); g->feedback.imu.gyro.z, 0.f, g->dt) + yaw_v_ff;
/* Pitch 轴：角度环 -> 速度环 + 前馈 */
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit, pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt); g->feedback.imu.eulr.rol, 0.0f, g->dt);
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) + pit_v_ff;
if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) {
g->out.pit=-g->out.pit;
}
break; break;
case GIMBAL_MODE_RELATIVE: case GIMBAL_MODE_RELATIVE:
/* 相对模式暂未实现 */
g->out.yaw = 0.0f; g->out.yaw = 0.0f;
g->out.pit = 0.0f; g->out.pit = 0.0f;
break; break;
@ -253,7 +254,134 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
return 0; return 0;
} }
/**
* \brief 运行云台控制逻辑（集成 AI 速度前馈与目标滤波）
*
* \param g 包含云台数据的结构体
* \param g_cmd 云台控制指令（包含遥控器增量及视觉数据）
*
* \return 函数运行结果
*/
int8_t Gimbal_Control1(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
if (g == NULL || g_cmd == NULL) {
return -1;
}
/* 更新时间步长 */
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
g->lask_wakeup = BSP_TIME_Get_us();
/* 模式切换处理 */
Gimbal_SetMode(g, g_cmd->mode);
/* --- 0. 视觉滤波器初始化与复位逻辑 --- */
if (!is_vision_lpf_init) {
/* 假设控制频率约为1000Hz(取决于实际dt)，截止频率设为15Hz。截止频率越低越平滑，但延迟越大 */
LowPassFilter2p_Init(&vision_yaw_lpf, 1.0f / g->dt, 15.0f);
LowPassFilter2p_Init(&vision_pit_lpf, 1.0f / g->dt, 15.0f);
is_vision_lpf_init = 1;
}
/* --- 1. 计算目标设定值增量 --- */
float delta_yaw = 0.0f;
float delta_pit = 0.0f;
if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) {
/* 对视觉传来的绝对角度进行二阶低通滤波 */
float filtered_vision_yaw = LowPassFilter2p_Apply(&vision_yaw_lpf, g_cmd->vision.yaw);
float filtered_vision_pit = LowPassFilter2p_Apply(&vision_pit_lpf, g_cmd->vision.pit);
/* Yaw轴：圆周差值计算 */
delta_yaw = CircleError(filtered_vision_yaw, g->setpoint.eulr.yaw, M_2PI);
/* Pitch轴：纯线性差值计算。根据前置测试，如果极性反了，在此处添加负号 */
delta_pit = filtered_vision_pit - g->setpoint.eulr.pit; // 如果反向，改为 (-filtered_vision_pit) - g->setpoint.eulr.pit;
} else {
/* 丢失目标或非自瞄模式：复位滤波器，防止下次切入自瞄时产生剧烈阶跃 */
LowPassFilter2p_Reset(&vision_yaw_lpf, g->setpoint.eulr.yaw);
LowPassFilter2p_Reset(&vision_pit_lpf, g->setpoint.eulr.pit);
/* 使用操作手遥控器增量 */
delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
delta_pit = g_cmd->delta_pit * g->dt;
}
/* --- 2. 软件限位处理 (Yaw) --- */
if (g->param->travel.yaw > 0) {
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
const float delta_max = CircleError(g->limit.yaw.max, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
const float delta_min = CircleError(g->limit.yaw.min, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
if (delta_yaw > delta_max) delta_yaw = delta_max;
if (delta_yaw < delta_min) delta_yaw = delta_min;
}
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
/* --- 3. 软件限位处理 (Pitch) --- */
/* Pitch 轴必须使用纯线性累加和限幅，严禁使用 CircleAdd */
g->setpoint.eulr.pit += delta_pit;
if (g->param->travel.pit > 0) {
float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
float target_abs_pit = g->setpoint.eulr.pit + motor_imu_offset;
if (target_abs_pit > g->limit.pit.max) {
g->setpoint.eulr.pit = g->limit.pit.max - motor_imu_offset;
} else if (target_abs_pit < g->limit.pit.min) {
g->setpoint.eulr.pit = g->limit.pit.min - motor_imu_offset;
}
}
/* --- 4. 闭环控制计算 (串级 PID + 前馈) --- */
float yaw_omega_set_point, pit_omega_set_point;
float yaw_v_ff = 0.0f, pit_v_ff = 0.0f;
switch (g->mode) {
case GIMBAL_MODE_RELAX:
g->out.yaw = 0.0f;
g->out.pit = 0.0f;
break;
case GIMBAL_MODE_AUTO_AIM:
if (g_cmd->vision.target_found) {
/* 注意：已删除原有的强行赋值 g->setpoint.eulr.pit=g_cmd->vision.pit; */
yaw_v_ff = g_cmd->vision.yaw_v_ff * 0.2f;
pit_v_ff = g_cmd->vision.pit_v_ff * 0.2f;
}
/* 顺延执行 PID 计算 */
case GIMBAL_MODE_ABSOLUTE:
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt) + yaw_v_ff;
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt) + pit_v_ff;
g->out.pit=-g->out.pit; g->out.pit=-g->out.pit;
/* 注意：已删除此处的 g->out.pit = -g->out.pit; 修正输出极性应在外部 Gimbal_Output 或 PID 计算环节统一处理 */
break;
case GIMBAL_MODE_RELATIVE:
g->out.yaw = 0.0f;
g->out.pit = 0.0f;
break;
}
/* 输出滤波 */
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
return 0;
}
/** /**
* \brief 云台输出 * \brief 云台输出
* *

9
User/module/gimbal.h
View File

@ -28,15 +28,6 @@ extern "C" {
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* 在 Gimbal_Vision_t 结构体中增加前馈成员 */
typedef struct {
uint8_t target_found;
float yaw;
float pit;
float yaw_v_ff; // Yaw 速度前馈
float pit_v_ff; // Pitch 速度前馈
// 如有需要可增加加速度前馈 a_ff
} Gimbal_Vision_t;
typedef enum { typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */

138
User/module/shoot.c
View File

@ -1,4 +1,3 @@
#include "shoot.h" #include "shoot.h"
#include <string.h> #include <string.h>
#include "component/filter.h" #include "component/filter.h"
@ -7,10 +6,14 @@
#include "bsp/time.h" #include "bsp/time.h"
#include "device/motor_rm.h" #include "device/motor_rm.h"
#include "module/config.h" #include "module/config.h"
static bool last_firecmd; static bool last_firecmd;
/**
* @brief 确保两个输出权重的和不超过 1.0f，用于限幅
*/
static inline void ScaleSumTo1(float *a, float *b) { static inline void ScaleSumTo1(float *a, float *b) {
float sum = *a + *b; float sum = fabsf(*a) + fabsf(*b);
if (sum > 1.0f) { if (sum > 1.0f) {
float scale = 1.0f / sum; float scale = 1.0f / sum;
*a *= scale; *a *= scale;
@ -18,11 +21,10 @@ static inline void ScaleSumTo1(float *a, float *b) {
} }
} }
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode) int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
{ {
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1;
} }
s->mode = mode; s->mode = mode;
return 0; return 0;
@ -31,7 +33,7 @@ int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
int8_t Shoot_ResetIntegral(Shoot_t *s) int8_t Shoot_ResetIntegral(Shoot_t *s)
{ {
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1;
} }
for(int i = 0; i < SHOOT_FRIC_NUM; i++) for(int i = 0; i < SHOOT_FRIC_NUM; i++)
{ {
@ -46,7 +48,7 @@ int8_t Shoot_ResetIntegral(Shoot_t *s)
int8_t Shoot_ResetCalu(Shoot_t *s) int8_t Shoot_ResetCalu(Shoot_t *s)
{ {
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1;
} }
for(int i = 0; i < SHOOT_FRIC_NUM; i++) for(int i = 0; i < SHOOT_FRIC_NUM; i++)
{ {
@ -65,7 +67,7 @@ int8_t Shoot_ResetCalu(Shoot_t *s)
int8_t Shoot_ResetOutput(Shoot_t *s) int8_t Shoot_ResetOutput(Shoot_t *s)
{ {
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1;
} }
for(int i = 0; i < SHOOT_FRIC_NUM; i++) for(int i = 0; i < SHOOT_FRIC_NUM; i++)
{ {
@ -83,17 +85,13 @@ int8_t Shoot_ResetOutput(Shoot_t *s)
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
{ {
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1;
} }
s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM; // 固定的归一化目标值
s->target_variable.target_rpm = 4000.0f;
return 0; return 0;
} }
/**
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
*
* \param s 包含发射数据的结构体
* \param num 需要发射的弹丸数量
*/
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 || s->shoot_Anglecalu.num_to_shoot == 0) { if (s == NULL || s->shoot_Anglecalu.num_to_shoot == 0) {
@ -103,8 +101,9 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{ {
s->shoot_Anglecalu.time_last_shoot = s->now; s->shoot_Anglecalu.time_last_shoot = s->now;
s->target_variable.target_angle += s->param->trig_step_angle; s->target_variable.target_angle += s->param->trig_step_angle;
// 角度限制在 [-PI, PI]
if(s->target_variable.target_angle > M_PI) s->target_variable.target_angle -= M_2PI; if(s->target_variable.target_angle > M_PI) s->target_variable.target_angle -= M_2PI;
else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI; else if(s->target_variable.target_angle < -M_PI) s->target_variable.target_angle += M_2PI;
s->shoot_Anglecalu.num_to_shoot--; s->shoot_Anglecalu.num_to_shoot--;
} }
return 0; return 0;
@ -113,7 +112,7 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq) int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
{ {
if (s == NULL || param == NULL || target_freq <= 0.0f) { if (s == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误 return -1;
} }
s->param = param; s->param = param;
@ -134,7 +133,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
memset(&s->shoot_Anglecalu, 0, sizeof(s->shoot_Anglecalu)); memset(&s->shoot_Anglecalu, 0, sizeof(s->shoot_Anglecalu));
memset(&s->output, 0, sizeof(s->output)); memset(&s->output, 0, sizeof(s->output));
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */ s->target_variable.target_rpm = 6000.0f;
return 0; return 0;
} }
@ -142,46 +141,41 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
int8_t Shoot_UpdateFeedback(Shoot_t *s) int8_t Shoot_UpdateFeedback(Shoot_t *s)
{ {
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1;
} }
float rpm_sum = 0.0f; float rpm_sum = 0.0f;
for(int i = 0; i < SHOOT_FRIC_NUM; i++) { for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
/* 更新摩擦电机反馈 */
MOTOR_RM_Update(&s->param->fric_motor_param[i]); MOTOR_RM_Update(&s->param->fric_motor_param[i]);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]); MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
if(motor_fed!=NULL) if(motor_fed != NULL) {
{
s->feedback.fric[i] = motor_fed->motor.feedback; s->feedback.fric[i] = motor_fed->motor.feedback;
} }
/* 滤波反馈rpm */
s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed); s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
/* 归一化rpm */
s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM; s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
if(s->feedback.fric_rpm[i] > 1.0f) s->feedback.fric_rpm[i] = 1.0f; if(s->feedback.fric_rpm[i] > 1.0f) s->feedback.fric_rpm[i] = 1.0f;
if(s->feedback.fric_rpm[i] < -1.0f) s->feedback.fric_rpm[i] = -1.0f; if(s->feedback.fric_rpm[i] < -1.0f) s->feedback.fric_rpm[i] = -1.0f;
/* 计算平均rpm */
rpm_sum += s->feedback.fric_rpm[i]; rpm_sum += s->feedback.fric_rpm[i];
} }
s->feedback.fric_avgrpm = rpm_sum / SHOOT_FRIC_NUM; s->feedback.fric_avgrpm = rpm_sum / SHOOT_FRIC_NUM;
/* 更新拨弹电机反馈 */
MOTOR_RM_Update(&s->param->trig_motor_param); MOTOR_RM_Update(&s->param->trig_motor_param);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param); MOTOR_RM_t *motor_fed_trig = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
if(motor_fed!=NULL) if(motor_fed_trig != NULL) {
{ s->feedback.trig = motor_fed_trig->feedback;
s->feedback.trig=motor_fed->feedback;
} }
/* 将多圈角度归化到单圈 (-M_PI, M_PI) */
s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle; s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π] s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI);
if (s->feedback.trig_angle_cicle > M_PI) { if (s->feedback.trig_angle_cicle > M_PI) {
s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π] s->feedback.trig_angle_cicle -= M_2PI;
} else if (s->feedback.trig_angle_cicle < -M_PI) { } else if (s->feedback.trig_angle_cicle < -M_PI) {
s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π] s->feedback.trig_angle_cicle += M_2PI;
} }
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed); s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM; s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM;
if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f; //如果单环效果好就删 if(s->feedback.trig_rpm > 1.0f) s->feedback.trig_rpm = 1.0f;
if(s->feedback.trig_rpm < -1.0f) s->feedback.trig_rpm = -1.0f; if(s->feedback.trig_rpm < -1.0f) s->feedback.trig_rpm = -1.0f;
s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed; s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
@ -191,15 +185,15 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s)
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd) int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
{ {
if (s == NULL || cmd == NULL) { if (s == NULL || cmd == NULL) {
return -1; // 参数错误 return -1;
} }
s->now = BSP_TIME_Get_us() / 1000000.0f; s->now = BSP_TIME_Get_us() / 1000000.0f;
s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f; s->dt = (float)(BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
s->lask_wakeup = BSP_TIME_Get_us(); s->lask_wakeup = BSP_TIME_Get_us();
s->online = cmd->online; s->online = cmd->online;
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
for(int i=0;i<SHOOT_FRIC_NUM;i++) if(!s->online){
{ for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
MOTOR_RM_Relax(&s->param->fric_motor_param[i]); MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
} }
MOTOR_RM_Relax(&s->param->trig_motor_param); MOTOR_RM_Relax(&s->param->trig_motor_param);
@ -207,19 +201,28 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
else { else {
switch(s->running_state) switch(s->running_state)
{ {
case SHOOT_STATE_IDLE:/*熄火等待*/ case SHOOT_STATE_IDLE:/* 停止状态：下发 0 电流，避免反电动势制动 */
for(int i = 0; i < SHOOT_FRIC_NUM; i++) for(int i = 0; i < SHOOT_FRIC_NUM; i++)
{ {
// 直接将输出清零，不走 PID 计算
s->output.out_follow[i] = 0.0f;
s->output.out_err[i] = 0.0f;
s->output.out_fric[i] = 0.0f;
s->output.lpfout_fric[i] = 0.0f;
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt); // 重置 PID 积分项，防止下一次启动时瞬间过流
s->output.out_fric[i]=s->output.out_follow[i]; PID_ResetIntegral(&s->pid.fric_follow[i]);
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); PID_ResetIntegral(&s->pid.fric_err[i]);
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], 0.0f);
} }
// 拨弹电机保持现有位置控制
s->output.outagl_trig = PID_Calc(&s->pid.trig, s->target_variable.target_angle, s->feedback.trig_angle_cicle, 0, s->dt); s->output.outagl_trig = PID_Calc(&s->pid.trig, s->target_variable.target_angle, s->feedback.trig_angle_cicle, 0, s->dt);
s->output.outomg_trig = PID_Calc(&s->pid.trig_omg, s->output.outagl_trig, s->feedback.trig_rpm, 0, s->dt); s->output.outomg_trig = PID_Calc(&s->pid.trig_omg, s->output.outagl_trig, s->feedback.trig_rpm, 0, s->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->trig_motor_param, s->output.outlpf_trig); MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(cmd->ready) if(cmd->ready)
{ {
Shoot_ResetCalu(s); Shoot_ResetCalu(s);
@ -228,42 +231,36 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
s->running_state = SHOOT_STATE_READY; s->running_state = SHOOT_STATE_READY;
} }
break; break;
case SHOOT_STATE_READY:/*准备射击*/
case SHOOT_STATE_READY:/* 准备射击：摩擦轮闭环加速 */
for(int i = 0; i < SHOOT_FRIC_NUM; i++) for(int i = 0; i < SHOOT_FRIC_NUM; i++)
{ /* 计算跟随输出、计算修正输出 */ {
s->output.out_follow[i] = PID_Calc(&s->pid.fric_follow[i], s->target_variable.target_rpm / MAX_FRIC_RPM, s->feedback.fric_rpm[i], 0, s->dt); s->output.out_follow[i] = PID_Calc(&s->pid.fric_follow[i], s->target_variable.target_rpm / MAX_FRIC_RPM, s->feedback.fric_rpm[i], 0, s->dt);
s->output.out_err[i] = PID_Calc(&s->pid.fric_err[i], s->feedback.fric_avgrpm, s->feedback.fric_rpm[i], 0, s->dt); s->output.out_err[i] = PID_Calc(&s->pid.fric_err[i], s->feedback.fric_avgrpm, s->feedback.fric_rpm[i], 0, s->dt);
/* 按比例缩放并加和输出 */
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]); ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i] = s->output.out_follow[i] + s->output.out_err[i]; s->output.out_fric[i] = s->output.out_follow[i] + s->output.out_err[i];
/* 滤波 */
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
/* 输出 */
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
} }
/* 拨弹电机输出 */
s->output.outagl_trig = PID_Calc(&s->pid.trig, s->target_variable.target_angle, s->feedback.trig_angle_cicle, 0, s->dt); s->output.outagl_trig = PID_Calc(&s->pid.trig, s->target_variable.target_angle, s->feedback.trig_angle_cicle, 0, s->dt);
s->output.outomg_trig = PID_Calc(&s->pid.trig_omg, s->output.outagl_trig, s->feedback.trig_rpm, 0, s->dt); s->output.outomg_trig = PID_Calc(&s->pid.trig_omg, s->output.outagl_trig, s->feedback.trig_rpm, 0, s->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->trig_motor_param, s->output.outlpf_trig); MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
/* 检查状态机 */
if(!cmd->ready) if(!cmd->ready)
{ {
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state = SHOOT_STATE_IDLE; s->running_state = SHOOT_STATE_IDLE;
} }
else if(last_firecmd == false && cmd->firecmd == true) else if(last_firecmd == false && cmd->firecmd == true)
{ {
Shoot_ResetOutput(s);
s->running_state = SHOOT_STATE_FIRE; s->running_state = SHOOT_STATE_FIRE;
s->shoot_Anglecalu.num_to_shoot += s->param->shot_burst_num; s->shoot_Anglecalu.num_to_shoot += s->param->shot_burst_num;
} }
break; break;
case SHOOT_STATE_FIRE:
case SHOOT_STATE_FIRE:/* 发射状态 */
Shoot_CaluTargetAngle(s, cmd); Shoot_CaluTargetAngle(s, cmd);
for(int i = 0; i < SHOOT_FRIC_NUM; i++) for(int i = 0; i < SHOOT_FRIC_NUM; i++)
{ {
@ -278,34 +275,25 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
s->output.outomg_trig = PID_Calc(&s->pid.trig_omg, s->output.outagl_trig, s->feedback.trig_rpm, 0, s->dt); s->output.outomg_trig = PID_Calc(&s->pid.trig_omg, s->output.outagl_trig, s->feedback.trig_rpm, 0, s->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->trig_motor_param, s->output.outlpf_trig); MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(!cmd->firecmd)
if(!cmd->firecmd && s->shoot_Anglecalu.num_to_shoot == 0)
{ {
Shoot_ResetOutput(s);
s->running_state = SHOOT_STATE_READY; s->running_state = SHOOT_STATE_READY;
} }
break; break;
default: default:
s->running_state = SHOOT_STATE_IDLE; s->running_state = SHOOT_STATE_IDLE;
break; break;
} }
} }
// if (!g_debug_motor_enable) {
// for(int i=0;i<SHOOT_FRIC_NUM;i++) {
// MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], 0.0f); // 摩擦轮强制 0 电流
// }
// MOTOR_RM_SetOutput(&s->param->trig_motor_param, 0.0f); // 拨弹电机强制 0 电流
// }
MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]); // 执行底层电机控制
for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
MOTOR_RM_Ctrl(&s->param->fric_motor_param[i]);
}
MOTOR_RM_Ctrl(&s->param->trig_motor_param);
last_firecmd = cmd->firecmd; last_firecmd = cmd->firecmd;
return 0; return 0;
} }

86
User/task/ai.c
View File

@ -19,7 +19,7 @@
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
extern uint8_t AI_mode; // AI 模式变量，供其他模块查询当前AI控制模式 extern uint8_t AI_mode; // AI 模式变量，供其他模块查询当前AI控制模式
#define MAX_RX_BUF_SIZE 128 // 或 (sizeof(VisionToGimbal_t) * 2)
// MCU 数据结构（MCU -> 上位机） // MCU 数据结构（MCU -> 上位机）
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
uint8_t head[2]; // {'M', 'R'} uint8_t head[2]; // {'M', 'R'}
@ -33,7 +33,7 @@ typedef struct __attribute__((packed)) {
uint16_t bullet_count; // 子弹累计发送次数 uint16_t bullet_count; // 子弹累计发送次数
uint16_t crc16; uint16_t crc16;
} GimbalToVision_t; } GimbalToVision_t;
Gimbal_Vision_t vision_data1;
// 确保结构体大小符合要求 (C11 标准) // 确保结构体大小符合要求 (C11 标准)
#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L #if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
_Static_assert(sizeof(GimbalToVision_t) <= 64, "GimbalToVision_t size exceeds 64 bytes"); _Static_assert(sizeof(GimbalToVision_t) <= 64, "GimbalToVision_t size exceeds 64 bytes");
@ -67,61 +67,64 @@ GimbalToVision_t tx_pkg;
/* USER FUNCTION BEGIN */ /* USER FUNCTION BEGIN */
/** /**
* @brief AI串口接收完成回调函数 * @brief AI串口空闲中断回调函数 (适配无参 BSP 架构)
* @note 验证包头与CRC16校验，校验通过后更新控制指令，并重新开启DMA接收
*/ */
static void AI_RxCpltCallback(void) { static void AI_RxIdleCallback(void) {
/* 检查包头是否为 'M' 和 'R' */ /* 1. 获取本次空闲中断触发时的实际接收长度 */
if (ai_rx_buf[0] == 'M' && ai_rx_buf[1] == 'R') { uint16_t Size = BSP_UART_GetRxCount(BSP_UART_AI, MAX_RX_BUF_SIZE);
/* 校验 CRC，确保数据包完整 */
if (CRC16_Verify(ai_rx_buf, sizeof(VisionToGimbal_t))) {
memcpy(&ai_rx_data, ai_rx_buf, sizeof(VisionToGimbal_t));
/* --- 务必初始化局部变量 --- */ /* 2. 必须手动停止当前 DMA，以便在函数末尾重新对齐接收指针 */
HAL_UART_DMAStop(BSP_UART_GetHandle(BSP_UART_AI));
/* 3. 长度校验：严格匹配结构体长度 */
if (Size != sizeof(VisionToGimbal_t)) {
goto RESTART_RX;
}
/* 4. 包头校验 */
if (ai_rx_buf[0] != 'M' || ai_rx_buf[1] != 'R') {
goto RESTART_RX;
}
/* 5. CRC校验 */
if (!CRC16_Verify(ai_rx_buf, Size)) {
goto RESTART_RX;
}
/* --- 下方保留原有的解析和入队逻辑 --- */
VisionToGimbal_t *p_rx_data = (VisionToGimbal_t *)ai_rx_buf;
Gimbal_Vision_t vision_data; Gimbal_Vision_t vision_data;
memset(&vision_data, 0, sizeof(Gimbal_Vision_t)); memset(&vision_data, 0, sizeof(Gimbal_Vision_t));
Shoot_CMD_t ai_shoot_cmd; uint8_t is_valid_packet = 1;
memset(&ai_shoot_cmd, 0, sizeof(Shoot_CMD_t)); // 防止野指针走火
/* --- 正确赋值视觉结构体 --- */ switch (p_rx_data->mode) {
switch (ai_rx_data.mode) { case 0:
case 0: /* 不控制 */
vision_data.target_found = 0; vision_data.target_found = 0;
// ai_shoot_cmd.mode = SHOOT_MODE_STOP; __NOP();
break; break;
case 1: /* 控制云台但不开火 */ case 1:
case 2:
vision_data.target_found = 1; vision_data.target_found = 1;
vision_data.yaw = ai_rx_data.yaw; vision_data.yaw = p_rx_data->yaw;
vision_data.pit = ai_rx_data.pitch; vision_data.pit = p_rx_data->pitch;
vision_data.yaw_v_ff = ai_rx_data.yaw_vel; vision_data.yaw_v_ff = p_rx_data->yaw_vel;
vision_data.pit_v_ff = ai_rx_data.pitch_vel; vision_data.pit_v_ff = p_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; break;
default: default:
vision_data.target_found = 0; is_valid_packet = 0;
break; break;
} }
/* 将视觉数据发送到专属队列，不与遥控器命令冲突 */ if (is_valid_packet) {
osMessageQueuePut(task_runtime.msgq.vision.data, &vision_data, 0, 0); osMessageQueuePut(task_runtime.msgq.vision.data, &vision_data, 0, 0);
vision_data1 = vision_data;
/* 将发射指令发送到发射队列 (确保枚举名称与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);
} }
RESTART_RX:
/* 6. 重新开启空闲中断定长接收 */
BSP_UART_Receive_IDLE(BSP_UART_AI, ai_rx_buf, MAX_RX_BUF_SIZE);
}
/** /**
* @brief 向上位机发送MCU状态数据 * @brief 向上位机发送MCU状态数据
*/ */
@ -177,7 +180,8 @@ void Task_ai(void *argument) {
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
/* 注册串口接收完成回调并启动第一次DMA接收 */ /* 注册串口接收完成回调并启动第一次DMA接收 */
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, AI_RxCpltCallback); // 假设 BSP 已添加 IDLE 中断回调对应的枚举 BSP_UART_IDLE_CB
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB, AI_RxIdleCallback);
BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(VisionToGimbal_t), true); BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(VisionToGimbal_t), true);
/* USER CODE INIT END */ /* USER CODE INIT END */

33
User/task/atti_esti.c
View File

@ -78,17 +78,35 @@ void Task_atti_esti(void *argument) {
BMI088_ParseAccl(&bmi088); BMI088_ParseAccl(&bmi088);
BMI088_ParseGyro(&bmi088); BMI088_ParseGyro(&bmi088);
/* 根据设备接收到的数据进行姿态解析 */ /* =================== 核心修复区域：原始数据重映射 =================== */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); /* 采用 typeof 定义临时变量，拷贝原始数据以保留 z 轴等未修改的数据 */
/* 注意：typeof 为 GCC 扩展特性，STM32 编译环境（Keil/CubeIDE）默认支持 */
__typeof__(bmi088.accl) mapped_accl = bmi088.accl;
__typeof__(bmi088.gyro) mapped_gyro = bmi088.gyro;
/* 根据解析出来的四元数计算欧拉角 */ /* 执行90度旋转映射 (假设顺时针90度: X_new = Y_old, Y_new = -X_old) */
mapped_accl.x = bmi088.accl.x;
mapped_accl.y = bmi088.accl.y;
mapped_gyro.x = bmi088.gyro.x;
mapped_gyro.y = bmi088.gyro.y;
/* 根据重映射后的加速度与角速度进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &mapped_accl, &mapped_gyro, &magn);
/* 根据解析出来的正确四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
osKernelUnlock(); osKernelUnlock();
/* ==================================================================== */
// --- 新增：将IMU数据（角速度与欧拉角）发送至云台控制任务的队列 --- // --- 修改：将IMU数据（正确的角速度与欧拉角）发送至云台控制任务的队列 ---
Gimbal_IMU_t gimbal_imu_to_send; Gimbal_IMU_t gimbal_imu_to_send;
gimbal_imu_to_send.gyro = bmi088.gyro; // 填充陀螺仪角速度
gimbal_imu_to_send.eulr = eulr_to_send; // 填充解算后的欧拉角 // 直接使用映射好的角速度，确保内环（角速度）与外环（角度）坐标系完全一致
gimbal_imu_to_send.gyro = mapped_gyro;
gimbal_imu_to_send.eulr = eulr_to_send;
// 发送至队列
osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_imu_to_send, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_imu_to_send, 0, 0);
// ----------------------------------------------------------------- // -----------------------------------------------------------------
@ -98,8 +116,9 @@ void Task_atti_esti(void *argument) {
// 0: 不等待，如果队列满了直接跳过（保证实时性） // 0: 不等待，如果队列满了直接跳过（保证实时性）
osMessageQueuePut(task_runtime.msgq.ai.quat, &gimbal_ahrs.quat, 0, 0); osMessageQueuePut(task_runtime.msgq.ai.quat, &gimbal_ahrs.quat, 0, 0);
// 如果 AI 任务还需要欧拉角，也可以发送欧拉角队列 // 如果 AI 任务还需要欧拉角，发送欧拉角队列
osMessageQueuePut(task_runtime.msgq.ai.eulr, &eulr_to_send, 0, 0); osMessageQueuePut(task_runtime.msgq.ai.eulr, &eulr_to_send, 0, 0);
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.5f, BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.5f,
bmi088.temp, 0.0f, 0.0f)); bmi088.temp, 0.0f, 0.0f));

34
User/task/ctrl_gimbal.c
View File

@ -19,6 +19,8 @@
Gimbal_t gimbal; Gimbal_t gimbal;
Gimbal_IMU_t gimbal_imu; Gimbal_IMU_t gimbal_imu;
Gimbal_CMD_t gimbal_cmd; Gimbal_CMD_t gimbal_cmd;
uint32_t last_vision_update_tick = 0;
extern Gimbal_Vision_t vision_data1;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -35,9 +37,9 @@ void Task_ctrl_gimbal(void *argument) {
static Gimbal_Vision_t last_vision_data = {0}; static Gimbal_Vision_t last_vision_data = {0};
/* 新增：记录最后一次收到有效视觉数据的时间戳 */ /* 新增：记录最后一次收到有效视觉数据的时间戳 */
static uint32_t last_vision_update_tick = 0; // static uint32_t last_vision_update_tick = 0;
/* 新增：视觉断线超时阈值，此处设置为 100ms */ /* 新增：视觉断线超时阈值，此处设置为 100ms */
const uint32_t vision_timeout_ticks = osKernelGetTickFreq() / 10; const uint32_t vision_timeout_ticks = osKernelGetTickFreq() / 100;
while (1) { while (1) {
tick += delay_tick; tick += delay_tick;
@ -51,20 +53,20 @@ void Task_ctrl_gimbal(void *argument) {
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
/* 3. 获取并缓存 AI 视觉信号，增加超时判断逻辑 */ /* 3. 获取并缓存 AI 视觉信号，增加超时判断逻辑 */
Gimbal_Vision_t current_vision; // Gimbal_Vision_t current_vision;
if (osMessageQueueGet(task_runtime.msgq.vision.data, &current_vision, NULL, 0) == osOK) { // if (osMessageQueueGet(task_runtime.msgq.vision.data, &current_vision, NULL, 0) == osOK) {
last_vision_data = current_vision; /* 有新视觉包时更新缓存 */ // last_vision_data = current_vision; /* 有新视觉包时更新缓存 */
last_vision_update_tick = osKernelGetTickCount(); /* 更新收到数据的时间戳 */ // last_vision_update_tick = osKernelGetTickCount(); /* 更新收到数据的时间戳 */
} else { // } else {
/* 未收到新数据时，检查是否超时断线 */ // /* 未收到新数据时，检查是否超时断线 */
if ((osKernelGetTickCount() - last_vision_update_tick) > vision_timeout_ticks) { // if ((osKernelGetTickCount() - last_vision_update_tick) > vision_timeout_ticks) {
last_vision_data.target_found = 0; /* 超时强制视为目标丢失 */ // last_vision_data.target_found = 0; /* 超时强制视为目标丢失 */
/* 将缓存的 yaw/pit 角度清零，避免残留脏数据干扰 */ // /* 将缓存的 yaw/pit 角度清零，避免残留脏数据干扰 */
last_vision_data.yaw = 0.0f; // last_vision_data.yaw = 0.0f;
last_vision_data.pit = 0.0f; // last_vision_data.pit = 0.0f;
} // }
} // }
last_vision_data = vision_data1;
/* 4. 强制将缓存的视觉数据挂载到当前的控制指令中 */ /* 4. 强制将缓存的视觉数据挂载到当前的控制指令中 */
gimbal_cmd.vision = last_vision_data; gimbal_cmd.vision = last_vision_data;

8
User/task/rc.c
View File

@ -64,14 +64,14 @@ void Task_rc(void *argument) {
break; break;
case DR16_SW_MID: case DR16_SW_MID:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 1.0f; cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 3.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 1.0f; cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 3.0f;
AI_mode = 0; /* AI 模式：空闲 */ AI_mode = 0; /* AI 模式：空闲 */
break; break;
case DR16_SW_DOWN: case DR16_SW_DOWN:
cmd_for_gimbal.mode = GIMBAL_MODE_AUTO_AIM; cmd_for_gimbal.mode = GIMBAL_MODE_AUTO_AIM;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 1.0f; cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 3.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 1.0f; cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 3.0f;
AI_mode = 1; /* AI 模式：自动瞄准 */ AI_mode = 1; /* AI 模式：自动瞄准 */
break; break;
default: default: