80 lines
3.0 KiB
C
80 lines
3.0 KiB
C
/*
|
||
ctrl_gimbal Task
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include "component/ahrs.h"
|
||
#include "module/gimbal.h"
|
||
#include "module/config.h"
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* USER STRUCT BEGIN */
|
||
Gimbal_t gimbal;
|
||
Gimbal_IMU_t gimbal_imu;
|
||
Gimbal_CMD_t gimbal_cmd;
|
||
/* USER STRUCT END */
|
||
|
||
/* Private function --------------------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
void Task_ctrl_gimbal(void *argument) {
|
||
(void)argument;
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_GIMBAL_FREQ;
|
||
osDelay(CTRL_GIMBAL_INIT_DELAY);
|
||
uint32_t tick = osKernelGetTickCount();
|
||
|
||
Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
|
||
|
||
/* 缓存最近一次收到的有效视觉数据 */
|
||
static Gimbal_Vision_t last_vision_data = {0};
|
||
|
||
/* 新增：记录最后一次收到有效视觉数据的时间戳 */
|
||
static uint32_t last_vision_update_tick = 0;
|
||
/* 新增：视觉断线超时阈值，此处设置为 100ms */
|
||
const uint32_t vision_timeout_ticks = osKernelGetTickFreq() / 10;
|
||
|
||
while (1) {
|
||
tick += delay_tick;
|
||
|
||
/* 1. 更新 IMU 数据 */
|
||
if (osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0) == osOK) {
|
||
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
|
||
}
|
||
|
||
/* 2. 更新基础控制指令（这会覆盖全局 gimbal_cmd，丢失其内部的 vision 数据） */
|
||
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
|
||
|
||
/* 3. 获取并缓存 AI 视觉信号，增加超时判断逻辑 */
|
||
Gimbal_Vision_t current_vision;
|
||
if (osMessageQueueGet(task_runtime.msgq.vision.data, ¤t_vision, NULL, 0) == osOK) {
|
||
last_vision_data = current_vision; /* 有新视觉包时更新缓存 */
|
||
last_vision_update_tick = osKernelGetTickCount(); /* 更新收到数据的时间戳 */
|
||
} else {
|
||
/* 未收到新数据时，检查是否超时断线 */
|
||
if ((osKernelGetTickCount() - last_vision_update_tick) > vision_timeout_ticks) {
|
||
last_vision_data.target_found = 0; /* 超时强制视为目标丢失 */
|
||
/* 将缓存的 yaw/pit 角度清零，避免残留脏数据干扰 */
|
||
last_vision_data.yaw = 0.0f;
|
||
last_vision_data.pit = 0.0f;
|
||
}
|
||
}
|
||
|
||
/* 4. 强制将缓存的视觉数据挂载到当前的控制指令中 */
|
||
gimbal_cmd.vision = last_vision_data;
|
||
|
||
/* 5. 执行云台计算与输出 */
|
||
Gimbal_UpdateFeedback(&gimbal);
|
||
osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
|
||
|
||
Gimbal_Control(&gimbal, &gimbal_cmd);
|
||
Gimbal_Output(&gimbal);
|
||
|
||
osDelayUntil(tick);
|
||
}
|
||
} |