添加命令任务

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

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); /* 运行结束，等待下一次唤醒 */
   }
   
 }

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); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

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

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"

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 */

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); /* 运行结束，等待下一次唤醒 */
   }
+  
 }

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,
 };

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
 }