From da38e37d0231d779d57e53f6b37d0dda2673c2b8 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Tue, 7 Oct 2025 16:12:17 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=91=BD=E4=BB=A4=E4=BB=BB?=
 =?UTF-8?q?=E5=8A=A1?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CMakeLists.txt          |  2 ++
 User/task/atti_esti.c   | 11 ++++++++---
 User/task/command.c     | 44 +++++++++++++++++++++++++++++++++++++++++
 User/task/config.yaml   |  7 +++++++
 User/task/ctrl_gimbal.c |  1 -
 User/task/init.c        |  1 +
 User/task/rc.c          |  6 ++++--
 User/task/user_task.c   |  5 +++++
 User/task/user_task.h   |  8 ++++++++
 9 files changed, 79 insertions(+), 6 deletions(-)
 create mode 100644 User/task/command.c

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b18eb1e..16dcd0a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,6 +63,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/component/limiter.c
     User/component/lqr.c
     User/component/pid.c
+    User/component/speed_planner.c
     User/component/user_math.c
     User/component/vmc.c
 
@@ -87,6 +88,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/task/ai.c
     User/task/atti_esti.c
     User/task/blink.c
+    User/task/command.c
     User/task/ctrl_chassis.c
     User/task/ctrl_gimbal.c
     User/task/ctrl_shoot.c
diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c
index 65e5fa7..2f669d7 100644
--- a/User/task/atti_esti.c
+++ b/User/task/atti_esti.c
@@ -1,10 +1,9 @@
 /*
     atti_esti Task
-    陀螺仪解算任务
+    
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "bsp/pwm.h"
@@ -42,15 +41,20 @@ void Task_atti_esti(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
   
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
+
   osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
 
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
   BMI088_Init(&bmi088,&cali_bmi088);
   AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
 
   /* USER CODE INIT END */
-
+  
   while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
     BMI088_WaitNew();
     BMI088_AcclStartDmaRecv();
@@ -80,6 +84,7 @@ void Task_atti_esti(void *argument) {
     osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
 
     /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
   
 }
\ No newline at end of file
diff --git a/User/task/command.c b/User/task/command.c
new file mode 100644
index 0000000..c93b319
--- /dev/null
+++ b/User/task/command.c
@@ -0,0 +1,44 @@
+/*
+    command Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_command(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / COMMAND_FREQ;
+
+  osDelay(COMMAND_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}
\ No newline at end of file
diff --git a/User/task/config.yaml b/User/task/config.yaml
index 8184c9d..d7b2264 100644
--- a/User/task/config.yaml
+++ b/User/task/config.yaml
@@ -54,3 +54,10 @@
   function: Task_ai
   name: ai
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_command
+  name: command
+  stack: 256
diff --git a/User/task/ctrl_gimbal.c b/User/task/ctrl_gimbal.c
index 9eed8a1..f795815 100644
--- a/User/task/ctrl_gimbal.c
+++ b/User/task/ctrl_gimbal.c
@@ -4,7 +4,6 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "component/ahrs.h"
diff --git a/User/task/init.c b/User/task/init.c
index 3fab228..ad183cf 100644
--- a/User/task/init.c
+++ b/User/task/init.c
@@ -41,6 +41,7 @@ void Task_Init(void *argument) {
   task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
   task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
   task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
+  task_runtime.thread.command = osThreadNew(Task_command, NULL, &attr_command);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
diff --git a/User/task/rc.c b/User/task/rc.c
index 76d28fc..9b4b14c 100644
--- a/User/task/rc.c
+++ b/User/task/rc.c
@@ -1,6 +1,6 @@
 /*
     rc Task
-
+    
 */
 
 /* Includes ----------------------------------------------------------------- */
@@ -34,6 +34,7 @@ Gimbal_CMD_t cmd_for_gimbal;
 void Task_rc(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
+  
   /* 计算任务运行到指定频率需要等待的tick数 */
   const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
 
@@ -43,7 +44,7 @@ void Task_rc(void *argument) {
   /* USER CODE INIT BEGIN */
   DR16_Init(&dr16);
   /* USER CODE INIT END */
-
+  
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
@@ -130,4 +131,5 @@ void Task_rc(void *argument) {
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
+  
 }
\ No newline at end of file
diff --git a/User/task/user_task.c b/User/task/user_task.c
index 02e9e51..d5928b6 100644
--- a/User/task/user_task.c
+++ b/User/task/user_task.c
@@ -48,4 +48,9 @@ const osThreadAttr_t attr_ai = {
     .name = "ai",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_command = {
+    .name = "command",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
 };
\ No newline at end of file
diff --git a/User/task/user_task.h b/User/task/user_task.h
index d10a055..ae5a6c8 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -21,6 +21,7 @@ extern "C" {
 #define CTRL_SHOOT_FREQ (500.0)
 #define MONITOR_FREQ (500.0)
 #define AI_FREQ (500.0)
+#define COMMAND_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -32,6 +33,7 @@ extern "C" {
 #define CTRL_SHOOT_INIT_DELAY (0)
 #define MONITOR_INIT_DELAY (0)
 #define AI_INIT_DELAY (0)
+#define COMMAND_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -49,6 +51,7 @@ typedef struct {
         osThreadId_t ctrl_shoot;
         osThreadId_t monitor;
         osThreadId_t ai;
+        osThreadId_t command;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -89,6 +92,7 @@ typedef struct {
         UBaseType_t ctrl_shoot;
         UBaseType_t monitor;
         UBaseType_t ai;
+        UBaseType_t command;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -101,6 +105,7 @@ typedef struct {
         float ctrl_shoot;
         float monitor;
         float ai;
+        float command;
     } freq;
 
     /* 任务最近运行时间 */
@@ -113,6 +118,7 @@ typedef struct {
         float ctrl_shoot;
         float monitor;
         float ai;
+        float command;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -130,6 +136,7 @@ extern const osThreadAttr_t attr_ctrl_gimbal;
 extern const osThreadAttr_t attr_ctrl_shoot;
 extern const osThreadAttr_t attr_monitor;
 extern const osThreadAttr_t attr_ai;
+extern const osThreadAttr_t attr_command;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -141,6 +148,7 @@ void Task_ctrl_gimbal(void *argument);
 void Task_ctrl_shoot(void *argument);
 void Task_monitor(void *argument);
 void Task_ai(void *argument);
+void Task_command(void *argument);
 
 #ifdef __cplusplus
 }