infantry

User/task/ai.c

@@ -0,0 +1,72 @@
+/*
+  AI上位机通信任务
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "device\ai.h"
+
+#include "bsp\usb.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+#ifdef DEBUG
+AI_t ai;
+CMD_Host_t cmd_host;
+AHRS_Quaternion_t quat;
+Referee_ForAI_t referee_ai;
+#else
+static AI_t ai;
+static CMD_Host_t cmd_host;
+static AHRS_Quaternion_t quat;
+static Referee_ForAI_t referee_ai;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief AI通信
+ *
+ * \param argument 未使用
+ */
+void Task_Ai(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_AI;
+
+  /* 初始化AI通信 */
+  AI_Init(&ai);
+
+  uint32_t tick = osKernelGetTickCount();
+  while (1) {
+#ifdef DEBUG
+    task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
+#endif
+    /* Task body */
+    tick += delay_tick;
+
+    AI_StartReceiving(&ai);
+    if (AI_WaitDmaCplt()) {
+      AI_ParseHost(&ai, &cmd_host);
+    } else {
+      AI_HandleOffline(&ai, &cmd_host);
+    }
+    osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
+    osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
+
+    osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
+    bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
+                                         &(referee_ai), NULL, 0) == osOK);
+    AI_PackMCU(&ai, &quat);
+    if (ref_update) AI_PackRef(&ai, &(referee_ai));
+
+    AI_StartSend(&(ai), ref_update);
+
+    osDelayUntil(tick);
+  }
+}

User/task/atti_esti.c

@@ -0,0 +1,138 @@
+/*
+  姿态解算任务。
+
+  控制IMU加热到指定温度防止温漂，收集IMU数据给AHRS算法。
+
+  收集BMI088的数据，解算后得到四元数，转换为欧拉角之后放到消息队列中，
+  等待其他任务取用。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include <string.h>
+
+#include "bsp\mm.h"
+#include "bsp\pwm.h"
+#include "bsp\usb.h"
+#include "component\ahrs.h"
+#include "component\pid.h"
+#include "device\bmi088.h"
+#include "device\ist8310.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+#ifdef DEBUG
+BMI088_t bmi088;
+IST8310_t ist8310;
+
+AHRS_t gimbal_ahrs;
+AHRS_Eulr_t eulr_to_send;
+
+KPID_t imu_temp_ctrl_pid;
+#else
+static BMI088_t bmi088;
+static IST8310_t ist8310;
+
+static AHRS_t gimbal_ahrs;
+static AHRS_Eulr_t eulr_to_send;
+
+static KPID_t imu_temp_ctrl_pid;
+#endif
+
+static const KPID_Params_t imu_temp_ctrl_pid_param = {
+    .k = 0.15f,
+    .p = 1.0f,
+    .i = 0.0f,
+    .d = 0.0f,
+    .i_limit = 1.0f,
+    .out_limit = 1.0f,
+};
+
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 姿态解算
+ *
+ * \param argument 未使用
+ */
+void Task_AttiEsti(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  /* 初始化设备 */
+  BMI088_Init(&bmi088, &(task_runtime.cfg.cali.bmi088));
+  // IST8310_Init(&ist8310, &(task_runtime.cfg.cali.ist8310));
+
+  /* 读取一次磁力计数据，用以初始化姿态解算算法 */
+  // IST8310_WaitNew(osWaitForever);
+  // IST8310_StartDmaRecv();
+  // IST8310_WaitDmaCplt();
+  // IST8310_Parse(&ist8310);
+
+  /* 初始化姿态解算算法 */
+  AHRS_Init(&gimbal_ahrs, &ist8310.magn, BMI088_GetUpdateFreq(&bmi088));
+
+  /* 初始化IMU温度控制PID，防止温漂 */
+  PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
+           1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
+
+  /* IMU温度控制PWM输出 */
+  BSP_PWM_Start(BSP_PWM_IMU_HEAT);
+
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.atti_esti =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+    /* 等待IMU新数据 */
+    BMI088_WaitNew();
+
+    /* 开始数据接收DMA，加速度计和陀螺仪共用同一个SPI接口，
+     * 一次只能开启一个DMA
+     */
+    BMI088_AcclStartDmaRecv();
+    BMI088_AcclWaitDmaCplt();
+
+    BMI088_GyroStartDmaRecv();
+    BMI088_GyroWaitDmaCplt();
+
+    /* 磁力计的数据接收频率远小于IMU，
+     * 这里使用非阻塞操作，保证姿态解算实时性
+     */
+    // IST8310_WaitNew(0);
+    // IST8310_StartDmaRecv();
+    // IST8310_WaitDmaCplt();
+
+    /* 锁住RTOS内核防止数据解析过程中断，造成错误 */
+    osKernelLock();
+    /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
+    BMI088_ParseAccl(&bmi088);
+    BMI088_ParseGyro(&bmi088);
+    // IST8310_Parse(&ist8310);
+
+    /* 根据设备接收到的数据进行姿态解析 */
+    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &ist8310.magn);
+
+    /* 根据解析出来的四元数计算欧拉角 */
+    AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+    osKernelUnlock();
+
+    /* 将需要与其他任务分享的数据放到消息队列中 */
+    osMessageQueueReset(task_runtime.msgq.gimbal.accl);
+    osMessageQueuePut(task_runtime.msgq.gimbal.accl, &bmi088.accl, 0, 0);
+    osMessageQueueReset(task_runtime.msgq.gimbal.eulr_imu);
+    osMessageQueuePut(task_runtime.msgq.gimbal.eulr_imu, &eulr_to_send, 0, 0);
+    osMessageQueueReset(task_runtime.msgq.ai.quat);
+    osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
+    osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
+    osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
+
+    /* PID控制IMU温度，PWM输出 */
+    BSP_PWM_Set(BSP_PWM_IMU_HEAT,
+                PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f));
+  }
+}

User/task/can.c

@@ -0,0 +1,93 @@
+/*
+  CAN总线数据处理
+
+  处理CAN总线收到的电机电容数据。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "device\can.h"
+
+#include "device\referee.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+#ifdef DEBUG
+CAN_t can;
+CAN_Output_t can_out;
+CAN_RawRx_t can_rx;
+#else
+static CAN_t can;
+static CAN_Output_t can_out;
+static CAN_RawRx_t can_rx;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_Can(void *argument) {
+  (void)argument;
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
+
+  /* Device Setup */
+  CAN_Init(&can, &task_runtime.cfg.robot_param->can);
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* Task Setup */
+  while (1) {
+#ifdef DEBUG
+    task_runtime.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
+#endif
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    while (osMessageQueueGet(can.msgq_raw, &can_rx, 0, 0) == osOK) {
+      CAN_StoreMsg(&can, &can_rx);
+    }
+    osMessageQueueReset(task_runtime.msgq.can.feedback.chassis);
+    osMessageQueuePut(task_runtime.msgq.can.feedback.chassis, &can, 0, 0);
+
+    osMessageQueueReset(task_runtime.msgq.can.feedback.gimbal);
+    osMessageQueuePut(task_runtime.msgq.can.feedback.gimbal, &can, 0, 0);
+
+    osMessageQueueReset(task_runtime.msgq.can.feedback.shoot);
+    osMessageQueuePut(task_runtime.msgq.can.feedback.shoot, &can, 0, 0);
+
+    if (CAN_CheckFlag(&can, CAN_REC_CAP_FINISHED)) {
+      osMessageQueueReset(task_runtime.msgq.can.feedback.cap);
+      osMessageQueuePut(task_runtime.msgq.can.feedback.cap, &can, 0, 0);
+      CAN_ClearFlag(&can, CAN_REC_CAP_FINISHED);
+    } else {
+      // Error Handle
+    }
+
+    if (CAN_CheckFlag(&can, CAN_REC_TOF_FINISHED)) {
+      osMessageQueueReset(task_runtime.msgq.can.feedback.tof);
+      osMessageQueuePut(task_runtime.msgq.can.feedback.tof, &can, 0, 0);
+      CAN_ClearFlag(&can, CAN_REC_TOF_FINISHED);
+    } else {
+      // Error Handle
+    }
+
+    if (osMessageQueueGet(task_runtime.msgq.can.output.chassis,
+                          &(can_out.chassis), 0, 0) == osOK) {
+      CAN_Motor_Control(CAN_MOTOR_GROUT_CHASSIS, &can_out, &can);
+    }
+
+    if (osMessageQueueGet(task_runtime.msgq.can.output.gimbal,
+                          &(can_out.gimbal), 0, 0) == osOK) {
+      CAN_Motor_Control(CAN_MOTOR_GROUT_GIMBAL1, &can_out, &can);
+    }
+
+    if (osMessageQueueGet(task_runtime.msgq.can.output.shoot, &(can_out.shoot),
+                          0, 0) == osOK) {
+      CAN_Motor_Control(CAN_MOTOR_GROUT_SHOOT1, &can_out, &can);
+    }
+
+    if (osMessageQueueGet(task_runtime.msgq.can.output.cap, &(can_out.cap), 0,
+                          0) == osOK) {
+      CAN_Cap_Control(&(can_out.cap), &can);
+    }
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/cap.c

@@ -0,0 +1,93 @@
+/*
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "module\cap.h"
+
+#include "device\referee.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+static CAN_t can;
+
+#ifdef DEBUG
+CAN_CapOutput_t cap_out;
+Referee_ForCap_t referee_cap;
+Referee_CapUI_t cap_ui;
+#else
+static CAN_CapOutput_t cap_out;
+static Referee_ForCap_t referee_cap;
+static Referee_CapUI_t cap_ui;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 控制电容
+ *
+ * \param argument 未使用
+ */
+void Task_Cap(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  uint32_t last_online_tick = osKernelGetTickCount();
+
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CAP;
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.cap = osThreadGetStackSpace(osThreadGetId());
+#endif
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+
+    /* 读取裁判系统信息 */
+    osMessageQueueGet(task_runtime.msgq.referee.cap, &referee_cap, 0, 0);
+
+    /* 一定时间长度内接收不到电容反馈值，使电容离线 */
+    if (osMessageQueueGet(task_runtime.msgq.can.feedback.cap, &can, NULL,
+                          delay_tick) != osOK) {
+      if (osKernelGetTickCount() - last_online_tick > 1000) {
+        CAN_CAP_HandleOffline(&(can.cap), &cap_out,
+                              CHASSIS_POWER_MAX_WITHOUT_REF);
+
+        osMessageQueueReset(task_runtime.msgq.can.output.cap);
+        osMessageQueuePut(task_runtime.msgq.can.output.cap, &cap_out, 0, 0);
+        osMessageQueueReset(task_runtime.msgq.cap_info);
+        osMessageQueuePut(task_runtime.msgq.cap_info, &(can.cap), 0, 0);
+
+        Cap_DumpUI(&(can.cap), &cap_ui);
+
+        osMessageQueueReset(task_runtime.msgq.ui.cap);
+        osMessageQueuePut(task_runtime.msgq.ui.cap, &cap_ui, 0, 0);
+      }
+    } else {
+      last_online_tick = osKernelGetTickCount();
+
+      osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+      /* 根据裁判系统数据计算输出功率 */
+      Cap_Control(&can.cap, &referee_cap, &cap_out);
+      osKernelUnlock();
+      /* 将电容输出值发送到CAN */
+      osMessageQueueReset(task_runtime.msgq.can.output.cap);
+      osMessageQueuePut(task_runtime.msgq.can.output.cap, &cap_out, 0, 0);
+      /* 将电容状态发送到Chassis */
+      osMessageQueueReset(task_runtime.msgq.cap_info);
+      osMessageQueuePut(task_runtime.msgq.cap_info, &(can.cap), 0, 0);
+
+      Cap_DumpUI(&(can.cap), &cap_ui);
+
+      osMessageQueueReset(task_runtime.msgq.ui.cap);
+      osMessageQueuePut(task_runtime.msgq.ui.cap, &cap_ui, 0, 0);
+
+      osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+    }
+  }
+}

User/task/cli.c

@@ -0,0 +1,744 @@
+/*
+  命令行交互界面（Command Line Interface）任务
+
+  实现命令行。
+
+  从USB虚拟串口读取数据，结果也打印到USB虚拟串口。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "bsp\can.h"
+#include "bsp\usb.h"
+#include "component\FreeRTOS_CLI.h"
+#include "task.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+typedef struct {
+  uint8_t stage;
+} FiniteStateMachine_t;
+
+/* Private define ----------------------------------------------------------- */
+#define MAX_INPUT_LENGTH 64
+
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+static const char *const CLI_WELCOME_MESSAGE =
+    "\r\n"
+//    "  ______         __           _______               __              \r\n"
+//    " |   __ \\.-----.|  |--.-----.|   |   |.---.-.-----.|  |_.-----.----.\r\n"
+//    " |      <|  _  ||  _  |  _  ||       ||  _  |__ --||   _|  -__|   _|\r\n"
+//    " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__|  \r\n"
+    " -------------------------------------------------------------------\r\n"
+    " FreeRTOS CLI. Type 'help' to view a list of registered commands.   \r\n"
+    "\r\n";
+
+/* Command示例 */
+static BaseType_t Command_Endian(char *out_buffer, size_t len,
+                                 const char *command_string) {
+  if (out_buffer == NULL) return pdFALSE;
+  (void)command_string; /* 没用到command_string，消除警告 */
+  len -= 1;             /* 字符串后面有\0 */
+
+  /* 任务本身相关的内容 */
+  uint8_t list[2] = {0x11, 0x22};
+  uint16_t force_convert = ((uint16_t *)list)[0];
+  uint16_t assembled = (uint16_t)(list[0] | (list[1] << 8));
+
+  static FiniteStateMachine_t fsm; /* 有限状态机 */
+  switch (fsm.stage) {
+    case 0:
+      /* 每个状态内只允许有一个snprintf相关函数，以保证安全 */
+      /* 每个snprintf相关函数必须带有长度限制 */
+      snprintf(out_buffer, len, "a[2] = {0x11, 0x22}\r\n");
+      fsm.stage++;   /* 改变状态机运行状态 */
+      return pdPASS; /* 需要继续运行下一状态时返回pdPASS */
+    case 1:
+      snprintf(out_buffer, len,
+               "Force convert to uint16 list, we got: 0x%x\r\n", force_convert);
+      fsm.stage++;
+      return pdPASS;
+    case 2:
+      snprintf(out_buffer, len,
+               "Manually assemble a[1], a[0], we got: 0x%x\r\n", assembled);
+      fsm.stage++;
+      return pdPASS;
+    case 3:
+      if (force_convert == assembled)
+        snprintf(out_buffer, len, "Small endian\r\n");
+      else
+        snprintf(out_buffer, len, "Big endian\r\n");
+      fsm.stage++;
+      return pdPASS;
+    default: /* 结束时状态 */
+      snprintf(out_buffer, len, "\r\n");
+      fsm.stage = 0;  /* 重置有限状态机 */
+      return pdFALSE; /* 不需要继续运行下一状态时返回pdFALSE */
+  }
+}
+
+static BaseType_t Command_Stats(char *out_buffer, size_t len,
+                                const char *command_string) {
+  static const char *const task_list_header =
+      "\r\n"
+      "-------------------Task list--------------------\r\n"
+      "Task          State  Priority  Stack\t#\r\n"
+      "------------------------------------------------\r\n";
+
+  static const char *const run_time_header =
+      "\r\n"
+      "-----------------Run time stats-----------------\r\n"
+      "Task            Abs Time        Time\r\n"
+      "------------------------------------------------\r\n";
+
+  static const char *const heap_header =
+      "\r\n"
+      "-------------------Heap stats-------------------\r\n"
+      "total(B)\tfree(B)\tused(B)\r\n"
+      "------------------------------------------------\r\n";
+
+  static const char *const hardware_header =
+      "\r\n"
+      "-----------------Hardware stats-----------------\r\n"
+      "CPU temp(C)\tBettary(V)\r\n"
+      "------------------------------------------------\r\n";
+
+  static const char *const config_header =
+      "\r\n"
+      "------------------Config stats------------------\r\n"
+      "Robot param\tPilot config\r\n"
+      "------------------------------------------------\r\n";
+
+  if (out_buffer == NULL) return pdFALSE;
+  (void)command_string;
+  len -= 1;
+
+  /* 堆区信息的相关内容 */
+  HeapStats_t heap_stats;
+
+  static FiniteStateMachine_t fsm;
+  switch (fsm.stage) {
+    case 0:
+      strncpy(out_buffer, task_list_header, len);
+      fsm.stage++;
+      return pdPASS;
+    case 1:
+      /* 获取任务的列表 */
+      vTaskList(out_buffer);
+      fsm.stage++;
+      return pdPASS;
+    case 2:
+      strncpy(out_buffer, run_time_header, len);
+      fsm.stage++;
+      return pdPASS;
+    case 3:
+      /* 获得任务的运行时间 */
+      vTaskGetRunTimeStats(out_buffer);
+      fsm.stage++;
+      return pdPASS;
+    case 4:
+      strncpy(out_buffer, heap_header, len);
+      fsm.stage++;
+      return pdPASS;
+    case 5:
+      /* 获得堆区的可用空间 */
+      vPortGetHeapStats(&heap_stats);
+      snprintf(out_buffer, len, "%d\t\t%d\t%d\r\n", configTOTAL_HEAP_SIZE,
+               heap_stats.xAvailableHeapSpaceInBytes,
+               configTOTAL_HEAP_SIZE - heap_stats.xAvailableHeapSpaceInBytes);
+      fsm.stage++;
+      return pdPASS;
+    case 6:
+      strncpy(out_buffer, hardware_header, len);
+      fsm.stage++;
+      return pdPASS;
+    case 7:
+      /* 获取当前CPU温度和电量 */
+      snprintf(out_buffer, len, "%f\t%f\r\n", task_runtime.status.cpu_temp,
+               task_runtime.status.battery);
+      fsm.stage++;
+      return pdPASS;
+    case 8:
+      strncpy(out_buffer, config_header, len);
+      fsm.stage++;
+      return pdPASS;
+    case 9:
+      /* 获取机器人和操作手名称 */
+      snprintf(out_buffer, len, "%s\t\t%s\r\n",
+               task_runtime.cfg.robot_param_name,
+               task_runtime.cfg.pilot_cfg_name);
+      fsm.stage++;
+      return pdPASS;
+    default:
+      snprintf(out_buffer, len, "\r\n");
+      fsm.stage = 0;
+      return pdFALSE;
+  }
+}
+
+static BaseType_t Command_Config(char *out_buffer, size_t len,
+                                 const char *command_string) {
+  /* 帮助信息，const保证不占用内存空间 */
+  static const char *const help_string =
+      "\r\n"
+      "usage: config <command> [<args>]\r\n"
+      "These are commands:\r\n"
+      "  help                     Display this message\r\n"
+      "  init                     Init config after flashing\r\n"
+      "  list <pilot/robot>       List available config\r\n"
+      "  set <pilot/robot> <name> Set config\r\n"
+      "\r\n";
+
+  /* 用常量表示状态机每个阶段，比数字更清楚 */
+  static const uint8_t stage_begin = 0;
+  static const uint8_t stage_success = 1;
+  static const uint8_t stage_end = 2;
+
+  if (out_buffer == NULL) return pdFALSE;
+  len -= 1;
+
+  /* 获取每一段参数的开始地址和长度 */
+  BaseType_t command_len, pr_len, name_len;
+  const char *command =
+      FreeRTOS_CLIGetParameter(command_string, 1, &command_len);
+  const char *pr = FreeRTOS_CLIGetParameter(command_string, 2, &pr_len);
+  const char *name = FreeRTOS_CLIGetParameter(command_string, 3, &name_len);
+
+  Config_t cfg;
+
+  static FiniteStateMachine_t fsm;
+  if (strncmp(command, "help", command_len) == 0) {
+    /* config help */
+    snprintf(out_buffer, len, "%s", help_string);
+    fsm.stage = stage_begin;
+    return pdFALSE;
+
+  } else if (strncmp(command, "init", command_len) == 0) {
+    if ((pr != NULL) || (name != NULL)) goto command_error;
+
+    /* config init */
+    switch (fsm.stage) {
+      case stage_begin:
+        snprintf(out_buffer, len, "\r\nSet Robot config to default and qs.");
+        fsm.stage = stage_success;
+        return pdPASS;
+
+      case stage_success:
+        /* 初始化获取的操作手配置、机器人参数 */
+        memset(&cfg, 0, sizeof(cfg));
+        cfg.pilot_cfg = Config_GetPilotCfg("qs");
+        cfg.robot_param = Config_GetRobotParam("default");
+        snprintf(cfg.robot_param_name, 20, "default");
+        snprintf(cfg.pilot_cfg_name, 20, "qs");
+        Config_Set(&cfg);
+        snprintf(out_buffer, len, "\r\nDone.");
+        fsm.stage = stage_end;
+        return pdPASS;
+    }
+  } else if (strncmp(command, "list", command_len) == 0) {
+    if ((pr == NULL) || (name != NULL)) goto command_error;
+
+    /* config list */
+    static uint8_t i = 0;
+
+    if (strncmp(pr, "pilot", pr_len) == 0) {
+      /* config list pilot */
+      const Config_PilotCfgMap_t *pilot = Config_GetPilotNameMap();
+      switch (fsm.stage) {
+        case stage_begin:
+          snprintf(out_buffer, len, "\r\nAvailable pilot cfg:");
+          fsm.stage = stage_success;
+          return pdPASS;
+        case stage_success:
+          if (pilot[i].name != NULL) {
+            snprintf(out_buffer, len, "\r\n  %s", pilot[i].name);
+            i++;
+            fsm.stage = stage_success;
+          } else {
+            i = 0;
+            fsm.stage = stage_end;
+          }
+          return pdPASS;
+      }
+    } else if (strncmp(pr, "robot", pr_len) == 0) {
+      /* config list robot */
+      const Config_RobotParamMap_t *robot = Config_GetRobotNameMap();
+      switch (fsm.stage) {
+        case stage_begin:
+          snprintf(out_buffer, len, "\r\nAvailable robot params:");
+          fsm.stage = stage_success;
+          return pdPASS;
+        case stage_success:
+          if (robot[i].name != NULL) {
+            snprintf(out_buffer, len, "\r\n  %s", robot[i].name);
+            i++;
+            fsm.stage = stage_success;
+          } else {
+            i = 0;
+            fsm.stage = stage_end;
+          }
+          return pdPASS;
+      }
+    } else {
+      goto command_error;
+    }
+
+  } else if (strncmp(command, "set", command_len) == 0) {
+    if ((pr == NULL) || (name == NULL)) goto command_error;
+
+    /* config set */
+    if (strncmp(pr, "robot", pr_len) == 0) {
+      /* config set robot */
+      if (fsm.stage == stage_begin) {
+        Config_Get(&cfg);
+        if (Config_GetRobotParam(name) == NULL) {
+          snprintf(out_buffer, len, "\r\nFailed: Unknow model.");
+          fsm.stage = stage_end;
+          return pdPASS;
+
+        } else {
+          snprintf(out_buffer, len, "\r\nSucceed.");
+          snprintf(cfg.robot_param_name, 20, "%s", name);
+          Config_Set(&cfg);
+          fsm.stage = stage_success;
+          return pdPASS;
+        }
+      }
+    } else if (strncmp(pr, "pilot", pr_len) == 0) {
+      /* config set pilot */
+      if (fsm.stage == 0) {
+        Config_Get(&cfg);
+        if (Config_GetPilotCfg(name) == NULL) {
+          snprintf(out_buffer, len, "\r\nFailed: Unknow pilot.");
+          fsm.stage = stage_end;
+          return pdPASS;
+        } else {
+          snprintf(out_buffer, len, "\r\nSucceed.");
+          snprintf(cfg.pilot_cfg_name, 20, "%s", name);
+          Config_Set(&cfg);
+          fsm.stage = stage_success;
+          return pdPASS;
+        }
+      }
+    } else {
+      goto command_error;
+    }
+    if (fsm.stage == stage_success) {
+      snprintf(out_buffer, len,
+               "\r\nRestart needed for setting to take effect.");
+      fsm.stage = stage_end;
+      return pdPASS;
+    }
+  } /* config set */
+
+  if (fsm.stage == stage_end) {
+    snprintf(out_buffer, len, "\r\n\r\n");
+    fsm.stage = stage_begin;
+    return pdFALSE;
+  }
+
+command_error:
+  snprintf(out_buffer, len,
+           "\r\nconfig: invalid command. See 'config help'.\r\n");
+  fsm.stage = stage_begin;
+  return pdFALSE;
+}
+
+static BaseType_t Command_CaliGyro(char *out_buffer, size_t len,
+                                   const char *command_string) {
+  if (out_buffer == NULL) return pdFALSE;
+  (void)command_string;
+  len -= 1;
+
+  /* 陀螺仪校准相关内容 */
+  Config_t cfg;
+  AHRS_Gyro_t gyro;
+  uint16_t count = 0;
+  static float x = 0.0f;
+  static float y = 0.0f;
+  static float z = 0.0f;
+  static uint8_t retry = 0;
+
+  static FiniteStateMachine_t fsm;
+  switch (fsm.stage) {
+    case 0:
+      snprintf(out_buffer, len, "\r\nStart gyroscope calibration.\r\n");
+      fsm.stage++;
+      return pdPASS;
+    case 1:
+      snprintf(out_buffer, len, "Please make the MCU stable.\r\n");
+      fsm.stage++;
+      return pdPASS;
+    case 2:
+      /* 无陀螺仪数据和校准重试超时时，校准失败 */
+      osThreadSuspend(task_runtime.thread.ctrl_gimbal);
+
+      if (osMessageQueueGet(task_runtime.msgq.gimbal.gyro, &gyro, NULL, 5) !=
+          osOK) {
+        snprintf(out_buffer, len, "Can not get gyro data.\r\n");
+        fsm.stage = 7;
+        osThreadResume(task_runtime.thread.ctrl_gimbal);
+
+        return pdPASS;
+      }
+      osThreadResume(task_runtime.thread.ctrl_gimbal);
+
+      if (BMI088_GyroStable(&gyro)) {
+        snprintf(out_buffer, len,
+                 "Controller is stable. Start calibation.\r\n");
+        fsm.stage++;
+        return pdPASS;
+      } else {
+        retry++;
+        if (retry < 3) {
+          snprintf(out_buffer, len, "Controller is unstable.\r\n");
+          fsm.stage = 1;
+        } else {
+          fsm.stage = 7;
+        }
+        return pdPASS;
+      }
+    case 3:
+      /* 记录1000组数据，求出平均值作为陀螺仪的3轴零偏 */
+      snprintf(out_buffer, len, "Calibation in progress.\r\n");
+      Config_Get(&cfg);
+      cfg.cali.bmi088.gyro_offset.x = 0.0f;
+      cfg.cali.bmi088.gyro_offset.y = 0.0f;
+      cfg.cali.bmi088.gyro_offset.z = 0.0f;
+      Config_Set(&cfg);
+      while (count < 1000) {
+        bool data_new = (osMessageQueueGet(task_runtime.msgq.gimbal.gyro, &gyro,
+                                           NULL, 5) == osOK);
+        bool data_good = BMI088_GyroStable(&gyro);
+        if (data_new && data_good) {
+          x += gyro.x;
+          y += gyro.y;
+          z += gyro.z;
+          count++;
+        }
+      }
+      x /= (float)count;
+      y /= (float)count;
+      z /= (float)count;
+      fsm.stage++;
+      return pdPASS;
+    case 4:
+      snprintf(out_buffer, len,
+               "Calibation finished. Write result to flash.\r\n");
+      Config_Get(&cfg);
+      cfg.cali.bmi088.gyro_offset.x = x;
+      cfg.cali.bmi088.gyro_offset.y = y;
+      cfg.cali.bmi088.gyro_offset.z = z;
+      Config_Set(&cfg);
+      fsm.stage++;
+      return pdPASS;
+    case 5:
+      snprintf(out_buffer, len, "x:%.5f; y:%.5f; z:%.5f;\r\n", x, y, z);
+      fsm.stage++;
+      return pdPASS;
+    case 6:
+      snprintf(out_buffer, len, "Calibation done.\r\n");
+      fsm.stage = 8;
+      return pdPASS;
+    case 7:
+      snprintf(out_buffer, len, "Calibation failed.\r\n");
+      fsm.stage = 8;
+      return pdPASS;
+    default:
+      x = 0.0f;
+      y = 0.0f;
+      z = 0.0f;
+      retry = 0;
+      snprintf(out_buffer, len, "\r\n");
+      fsm.stage = 0;
+      return pdFALSE;
+  }
+}
+
+static BaseType_t Command_SetMechZero(char *out_buffer, size_t len,
+                                      const char *command_string) {
+  if (out_buffer == NULL) return pdFALSE;
+  (void)command_string;
+  len -= 1;
+
+  CAN_t can;
+  Config_t cfg;
+
+  static FiniteStateMachine_t fsm;
+  switch (fsm.stage) {
+    case 0:
+      snprintf(out_buffer, len, "\r\nStart setting mechanical zero point.\r\n");
+      fsm.stage++;
+      return pdPASS;
+    case 1:
+      /* 获取到云台数据，用can上的新的云台机械零点的位置替代旧的位置 */
+      Config_Get(&cfg);
+
+      osThreadSuspend(task_runtime.thread.ctrl_gimbal);
+      if (osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
+                            5) != osOK) {
+        snprintf(out_buffer, len, "Can not get gimbal data.\r\n");
+        fsm.stage = 2;
+        osThreadResume(task_runtime.thread.ctrl_gimbal);
+        return pdPASS;
+      }
+      osThreadResume(task_runtime.thread.ctrl_gimbal);
+      cfg.mech_zero.yaw = can.motor.gimbal.named.yaw.rotor_angle;
+      cfg.mech_zero.pit = can.motor.gimbal.named.pit.rotor_angle;
+
+      Config_Set(&cfg);
+      snprintf(out_buffer, len, "yaw:%f, pitch:%f, rol:%f\r\nDone.",
+               cfg.mech_zero.yaw, cfg.mech_zero.pit, cfg.mech_zero.rol);
+
+      fsm.stage = 3;
+      return pdPASS;
+    case 2:
+      snprintf(out_buffer, len, "Calibation failed.\r\n");
+      fsm.stage = 3;
+      return pdPASS;
+    default:
+      snprintf(out_buffer, len, "\r\n");
+      fsm.stage = 0;
+      return pdFALSE;
+  }
+}
+
+static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
+                                       const char *command_string) {
+  if (out_buffer == NULL) return pdFALSE;
+  (void)command_string;
+  len -= 1;
+
+  CAN_t can;
+  Config_t cfg;
+
+  static FiniteStateMachine_t fsm;
+  switch (fsm.stage) {
+    case 0:
+      Config_Get(&cfg);
+      snprintf(out_buffer, len, "(old)limit:%f\r\n", cfg.gimbal_limit);
+      fsm.stage = 1;
+      return pdPASS;
+    case 1:
+      /* 获取云台数据，获取新的限位角并替代旧的限位角 */
+      osThreadSuspend(task_runtime.thread.ctrl_gimbal);
+
+      if (osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
+                            10) != osOK) {
+        osThreadResume(task_runtime.thread.ctrl_gimbal);
+        fsm.stage = 3;
+        return pdPASS;
+      }
+      Config_Get(&cfg);
+      osThreadResume(task_runtime.thread.ctrl_gimbal);
+      cfg.gimbal_limit = can.motor.gimbal.named.pit.rotor_angle;
+
+      Config_Set(&cfg);
+      Config_Get(&cfg);
+      snprintf(out_buffer, len, "(new)limit:%f\r\nDone.", cfg.gimbal_limit);
+
+      fsm.stage = 2;
+      return pdPASS;
+    case 2:
+      snprintf(out_buffer, len,
+               "\r\nRestart needed for setting to take effect.");
+      fsm.stage = 5;
+      return pdPASS;
+    case 3:
+      snprintf(out_buffer, len,
+               "Calibation failed.\r\nCan not get gimbal data.\r\n");
+      fsm.stage = 5;
+      return pdPASS;
+    case 4:
+      snprintf(out_buffer, len, "Unknow options.\r\nPlease check help.");
+      fsm.stage = 5;
+      return pdPASS;
+    default:
+      snprintf(out_buffer, len, "\r\n");
+      fsm.stage = 0;
+      return pdFALSE;
+  }
+}
+
+/*
+static BaseType_t Command_XXX(char *out_buffer, size_t len,
+                                           const char *command_string) {
+  if (out_buffer == NULL) return pdFALSE;
+  (void)command_string;
+  len -= 1;
+
+  static FiniteStateMachine_t fsm;
+  switch (fsm.stage) {
+    case 0:
+
+      snprintf(out_buffer, len, "\r\nXXX.\r\n");
+
+
+      fsm.stage++;
+      return pdPASS;
+    case 1:
+      XXX();
+      snprintf(out_buffer, len, "\r\nDone.");
+
+
+      fsm.stage++;
+      return pdPASS;
+
+    default:
+      snprintf(out_buffer, len, "\r\n");
+      fsm.stage = 0;
+      return pdFALSE;
+  }
+}
+*/
+
+static const CLI_Command_Definition_t command_table[] = {
+    {
+        "endian",
+        "\r\nendian:\r\n Endian experiment.\r\n\r\n",
+        Command_Endian,
+        0,
+    },
+    {
+        "stats",
+        "\r\nstats:\r\n Displays a table showing the state of "
+        "FreeRTOS\r\n\r\n",
+        Command_Stats,
+        0,
+    },
+    {
+        "config",
+        "\r\nconfig:\r\n See 'config help'. \r\n\r\n",
+        Command_Config,
+        -1,
+    },
+    {
+        "cali-gyro",
+        "\r\ncali-gyro:\r\n Calibrate gyroscope. Remove zero offset.\r\n\r\n",
+        Command_CaliGyro,
+        0,
+    },
+    {
+        "set-mech-zero",
+        "\r\nset-mech-zero:\r\n Set mechanical zero point for gimbal.\r\n\r\n",
+        Command_SetMechZero,
+        0,
+    },
+    {
+        "set-gimbal-limit",
+        "\r\nset-gimbal-limit:\r\n Move the gimbal to the peak and execute this "
+        "command to calibrate the limit of gimbal.\r\n\r\n",
+        Command_SetGimbalLim,
+        0,
+    },
+    /*
+    {
+        "xxx-xxx",
+        "\r\nxxx-xxx:\r\n how to use. ",
+        Command_XXX,
+        1,
+    },
+    */
+};
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 命令行交互界面
+ *
+ * \param argument 未使用
+ */
+void Task_CLI(void *argument) {
+  (void)argument;                      /* 未使用argument，消除警告 */
+  static char input[MAX_INPUT_LENGTH]; /* 输入字符串缓存 */
+  char *output = FreeRTOS_CLIGetOutputBuffer(); /* 输出字符串缓存 */
+  char rx_char;                                 /* 接收到的字符 */
+  uint16_t index = 0;                           /* 字符串索引值 */
+  BaseType_t processing = 0;                    /* 命令行解析控制 */
+
+  /* 注册所有命令 */
+  const size_t num_commands =
+      sizeof(command_table) / sizeof(CLI_Command_Definition_t);
+  for (size_t j = 0; j < num_commands; j++) {
+    FreeRTOS_CLIRegisterCommand(command_table + j);
+  }
+
+  /* 通过回车键唤醒命令行界面 */
+  BSP_USB_Printf("Please press ENTER to activate this console.\r\n");
+  while (1) {
+    /* 等待接收到新的字符 */
+    BSP_USB_ReadyReceive(osThreadGetId());
+    osThreadFlagsWait(SIGNAL_BSP_USB_BUF_RECV, osFlagsWaitAll, osWaitForever);
+
+    /* 读取接收到的新字符 */
+    rx_char = BSP_USB_ReadChar();
+
+    /* 进行判断 */
+    if (rx_char == '\n' || rx_char == '\r') {
+      BSP_USB_Printf("%c", rx_char);
+      break;
+    }
+  }
+
+  /* 打印欢迎信息 */
+  BSP_USB_Printf(CLI_WELCOME_MESSAGE);
+
+  /* 开始运行命令行界面 */
+  BSP_USB_Printf("rm>");
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.cli = osThreadGetStackSpace(osThreadGetId());
+#endif
+    /* 等待输入. */
+    BSP_USB_ReadyReceive(osThreadGetId());
+    osThreadFlagsWait(SIGNAL_BSP_USB_BUF_RECV, osFlagsWaitAll, osWaitForever);
+
+    /* 读取接收到的新字符 */
+    rx_char = BSP_USB_ReadChar();
+
+    if (rx_char <= 126 && rx_char >= 32) {
+      /* 如果字符是可显示字符，则直接显式，并存入输入缓存中 */
+      if (index < MAX_INPUT_LENGTH) {
+        BSP_USB_Printf("%c", rx_char);
+        input[index] = rx_char;
+        index++;
+      }
+    } else {
+      /* 如果字符是控制字符，则需要进一步判断 */
+      if (rx_char == '\n' || rx_char == '\r') {
+        /* 如果输入的是回车，则认为命令输入完毕，进行下一步的解析和运行命令 */
+        BSP_USB_Printf("\r\n");
+        if (index > 0) {
+          /* 只在输入缓存有内容时起效 */
+          do {
+            /* 处理命令 */
+            processing = FreeRTOS_CLIProcessCommand(
+                input, output, configCOMMAND_INT_MAX_OUTPUT_SIZE);
+
+            BSP_USB_Printf(output);               /* 打印结果 */
+            memset(output, 0x00, strlen(output)); /* 清空输出缓存 */
+          } while (processing != pdFALSE); /* 是否需要重复运行命令 */
+          index = 0; /* 重置索引，准备接收下一段命令 */
+          memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
+        }
+        BSP_USB_Printf("rm>");
+      } else if (rx_char == '\b' || rx_char == 0x7Fu) {
+        /* 如果输入的是退格键则清空一位输入缓存，同时进行界限保护 */
+        if (index > 0) {
+          BSP_USB_Printf("%c", rx_char);
+          index--;
+          input[index] = 0;
+        }
+      }
+    }
+  }
+}

User/task/command.c

@@ -0,0 +1,98 @@
+/*
+  命令接收任务。
+
+  接收机器人的控制指令。
+
+  从DR16中接收数据，转换为通用的CMD_RC_t控制信号。
+  根据CMD_RC_t计算最终命令CMD_t。
+  把计算好的CMD_t细分后放到对应的消息队列中。
+  超时未收到则认为是丢控等特殊情况，把CMD_RC_t中的内容置零，
+  在后续的CMD_Parse中会根据此内容发现错误，保证机器人不失控。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include <string.h>
+
+#include "device\dr16.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+#ifdef DEBUG
+CMD_RC_t rc;
+CMD_Host_t host;
+CMD_t cmd;
+#else
+static CMD_RC_t rc;
+static CMD_Host_t host;
+static CMD_t cmd;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 控制指令接收
+ *
+ * \param argument 未使用
+ */
+void Task_Command(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_COMMAND;
+
+  /* 初始化指令处理 */
+  CMD_Init(&cmd, &(task_runtime.cfg.pilot_cfg->param));
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+
+  /* 用于计算遥控器数据频率 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.command =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+
+    osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+
+    /* 将接收机数据解析为指令数据 */
+    if (osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc, 0, 0) == osOK)
+      CMD_ParseRc(&rc, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
+
+    /* 判断是否需要让上位机覆写指令 */
+    if (CMD_CheckHostOverwrite(&cmd))
+      if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
+          osOK)
+        CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
+
+    osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+
+    /* 将需要与其他任务分享的数据放到消息队列中 */
+    osMessageQueueReset(task_runtime.msgq.cmd.ai);
+    osMessageQueuePut(task_runtime.msgq.cmd.ai, &(cmd.ai_status), 0, 0);
+
+    osMessageQueueReset(task_runtime.msgq.cmd.chassis);
+    osMessageQueuePut(task_runtime.msgq.cmd.chassis, &(cmd.chassis), 0, 0);
+
+    osMessageQueueReset(task_runtime.msgq.cmd.gimbal);
+    osMessageQueuePut(task_runtime.msgq.cmd.gimbal, &(cmd.gimbal), 0, 0);
+
+    osMessageQueueReset(task_runtime.msgq.cmd.shoot);
+    osMessageQueuePut(task_runtime.msgq.cmd.shoot, &(cmd.shoot), 0, 0);
+
+    osMessageQueueReset(task_runtime.msgq.ui.cmd);
+    osMessageQueuePut(task_runtime.msgq.ui.cmd, &cmd.pc_ctrl, 0, 0);
+
+    /* 存在裁判系统发送命令时，将相应的画图命令放入消息队列中 */
+    while (cmd.referee.counter > 0) {
+      osMessageQueuePut(task_runtime.msgq.cmd.referee,
+                        &(cmd.referee.cmd[--cmd.referee.counter]), 0, 0);
+      cmd.referee.cmd[cmd.referee.counter] = CMD_UI_NOTHING;
+    }
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/ctrl_chassis.c

@@ -0,0 +1,88 @@
+/*
+  底盘控制任务
+
+  控制底盘行为。
+
+  从CAN总线接收底盘电机反馈，根据接收到的控制命令，控制电机输出。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "component\limiter.h"
+#include "module\chassis.h"
+#include "module\config.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+static CAN_t can;
+#ifdef DEBUG
+CMD_ChassisCmd_t chassis_cmd;
+Chassis_t chassis;
+CAN_ChassisOutput_t chassis_out;
+CAN_Capacitor_t cap;
+Referee_ForChassis_t referee_chassis;
+Referee_ChassisUI_t chassis_ui;
+#else
+static CMD_ChassisCmd_t chassis_cmd;
+static Chassis_t chassis;
+static CAN_ChassisOutput_t chassis_out;
+static CAN_Capacitor_t cap;
+static Referee_ForChassis_t referee_chassis;
+static Referee_ChassisUI_t chassis_ui;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 控制底盘
+ *
+ * \param argument 未使用
+ */
+void Task_CtrlChassis(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CHASSIS;
+  /* 初始化底盘 */
+  Chassis_Init(&chassis, &(task_runtime.cfg.robot_param->chassis),
+               &task_runtime.cfg.mech_zero, (float)TASK_FREQ_CTRL_CHASSIS);
+
+  /* 延时一段时间再开启任务 */
+  osMessageQueueGet(task_runtime.msgq.can.feedback.chassis, &can, NULL,
+                    osWaitForever);
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.ctrl_chassis =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+
+    osMessageQueueGet(task_runtime.msgq.can.feedback.chassis, &can, NULL, 0);
+    /* 读取控制指令、电容反馈、裁判系统 */
+    osMessageQueueGet(task_runtime.msgq.referee.chassis, &referee_chassis, NULL,
+                      0);
+    osMessageQueueGet(task_runtime.msgq.cmd.chassis, &chassis_cmd, NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.cap_info, &cap, NULL, 0);
+    osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+    Chassis_UpdateFeedback(&chassis, &can); /* 更新反馈值 */
+    /* 根据遥控器命令计算底盘输出 */
+    Chassis_Control(&chassis, &chassis_cmd, tick);
+		/* PowerLimit_off */
+    Chassis_PowerLimit(&chassis, &cap, &referee_chassis); /* 限制输出功率 */
+    Chassis_DumpOutput(&chassis, &chassis_out);
+    osKernelUnlock();
+    /* 将电机输出值发送到CAN */
+    osMessageQueueReset(task_runtime.msgq.can.output.chassis);
+    osMessageQueuePut(task_runtime.msgq.can.output.chassis, &chassis_out, 0, 0);
+    /* 将底盘数据发送给UI */
+    Chassis_DumpUI(&chassis, &chassis_ui);
+    osMessageQueueReset(task_runtime.msgq.ui.chassis);
+    osMessageQueuePut(task_runtime.msgq.ui.chassis, &chassis_ui, 0, 0);
+
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/ctrl_gimbal.c

@@ -0,0 +1,83 @@
+/*
+  云台控制任务
+
+  控制云台行为。
+
+  从CAN总线接收底盘电机反馈，从IMU接收欧拉角和角速度，
+  根据接收到的控制命令，控制电机输出。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "module\gimbal.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+static CAN_t can;
+
+#ifdef DEBUG
+CMD_GimbalCmd_t gimbal_cmd;
+Gimbal_t gimbal;
+CAN_GimbalOutput_t gimbal_out;
+Referee_GimbalUI_t gimbal_ui;
+#else
+static CMD_GimbalCmd_t gimbal_cmd;
+static Gimbal_t gimbal;
+static CAN_GimbalOutput_t gimbal_out;
+static Referee_GimbalUI_t gimbal_ui;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 控制云台
+ *
+ * \param argument 未使用
+ */
+void Task_CtrlGimbal(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
+  /* 初始化云台 */
+  Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal),
+              task_runtime.cfg.gimbal_limit, (float)TASK_FREQ_CTRL_GIMBAL);
+
+  /* 延时一段时间再开启任务 */
+  osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
+                    osWaitForever);
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.ctrl_gimbal =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+
+    osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL, 0);
+    /* 读取控制指令、姿态、IMU数据 */
+    osMessageQueueGet(task_runtime.msgq.gimbal.eulr_imu,
+                      &(gimbal.feedback.eulr.imu), NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.gimbal.gyro, &(gimbal.feedback.gyro),
+                      NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.cmd.gimbal, &gimbal_cmd, NULL, 0);
+
+    osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+    Gimbal_UpdateFeedback(&gimbal, &can);
+    Gimbal_Control(&gimbal, &gimbal_cmd, tick);
+    Gimbal_DumpOutput(&gimbal, &gimbal_out);
+
+    osKernelUnlock();
+    osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
+    osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
+
+    Gimbal_DumpUI(&gimbal, &gimbal_ui);
+    osMessageQueueReset(task_runtime.msgq.ui.gimbal);
+    osMessageQueuePut(task_runtime.msgq.ui.gimbal, &gimbal_ui, 0, 0);
+
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/ctrl_shoot.c

@@ -0,0 +1,86 @@
+/*
+  射击控制任务
+
+  控制射击行为。
+
+  从CAN总线接收底盘电机反馈，根据接收到的控制命令，控制电机输出。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "module\shoot.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+static CAN_t can;
+
+#ifdef DEBUG
+CMD_ShootCmd_t shoot_cmd;
+Shoot_t shoot;
+Referee_ForShoot_t referee_shoot;
+CAN_ShootOutput_t shoot_out;
+Referee_ShootUI_t shoot_ui;
+#else
+static CMD_ShootCmd_t shoot_cmd;
+static Shoot_t shoot;
+static Referee_ForShoot_t referee_shoot;
+static CAN_ShootOutput_t shoot_out;
+static Referee_ShootUI_t shoot_ui;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 控制射击
+ *
+ * \param argument 未使用
+ */
+void Task_CtrlShoot(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT;
+  /* 初始化射击 */
+  Shoot_Init(&shoot, &(task_runtime.cfg.robot_param->shoot),
+             (float)TASK_FREQ_CTRL_SHOOT);
+
+  /* 延时一段时间再开启任务 */
+  osMessageQueueGet(task_runtime.msgq.can.feedback.shoot, &can, NULL,
+                    osWaitForever);
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.ctrl_shoot =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+    if (osMessageQueueGet(task_runtime.msgq.can.feedback.shoot, &can, NULL,
+                          0) != osOK) {
+      // Error handler
+      Shoot_ResetOutput(&shoot_out);
+    } else {
+      /* 读取控制指令以及裁判系统信息 */
+      osMessageQueueGet(task_runtime.msgq.cmd.shoot, &shoot_cmd, NULL, 0);
+      osMessageQueueGet(task_runtime.msgq.referee.shoot, &referee_shoot, NULL,
+                        0);
+      osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+      Shoot_UpdateFeedback(&shoot, &can);
+      /* 根据指令控制射击 */
+      Shoot_Control(&shoot, &shoot_cmd, &referee_shoot, HAL_GetTick());
+      /* 复制射击输出值 */
+      Shoot_DumpOutput(&shoot, &shoot_out);
+      osKernelUnlock();
+    }
+    osMessageQueueReset(task_runtime.msgq.can.output.shoot);
+    osMessageQueuePut(task_runtime.msgq.can.output.shoot, &shoot_out, 0, 0);
+
+    Shoot_DumpUI(&shoot, &shoot_ui);
+    osMessageQueueReset(task_runtime.msgq.ui.shoot);
+    osMessageQueuePut(task_runtime.msgq.ui.shoot, &shoot_ui, 0, 0);
+
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/info.c

@@ -0,0 +1,46 @@
+/*
+  消息任务
+
+  控制对外的指示装置，例如LED、OLED显示器等。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\led.h"
+#include "bsp\usb.h"
+#include "component\capacity.h"
+#include "component\user_math.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 信息
+ *
+ * \param argument 未使用
+ */
+void Task_Info(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_INFO;
+
+  osDelay(TASK_INIT_DELAY_INFO); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.info = osThreadGetStackSpace(osThreadGetId());
+#endif
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+
+    BSP_LED_Set(BSP_LED_GRN, BSP_LED_TAGGLE, 1); /* 闪烁LED */
+
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/init.c

@@ -0,0 +1,130 @@
+/*
+  初始化任务
+
+  根据机器人的FLASH配置，决定生成哪些任务。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\flash.h"
+#include "bsp\usb.h"
+#include "component\cmd.h"
+#include "device\bmi088.h"
+#include "device\can.h"
+#include "device\ist8310.h"
+#include "device\referee.h"
+#include "module\cap.h"
+#include "module\chassis.h"
+#include "module\gimbal.h"
+#include "module\shoot.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 初始化
+ *
+ * \param argument 未使用
+ */
+void Task_Init(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
+
+  osKernelLock();
+  /* 创建任务 */
+  task_runtime.thread.atti_esti =
+      osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
+  task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
+  task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
+  task_runtime.thread.ctrl_chassis =
+      osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
+  task_runtime.thread.ctrl_gimbal =
+      osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
+  task_runtime.thread.ctrl_shoot =
+      osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
+  task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
+  task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
+  task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
+  task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
+  task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
+  task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
+  task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
+
+  /* 创建消息队列 */
+  /* motor */
+  task_runtime.msgq.can.feedback.chassis =
+      osMessageQueueNew(2u, sizeof(CAN_t), NULL);
+  task_runtime.msgq.can.feedback.gimbal =
+      osMessageQueueNew(2u, sizeof(CAN_t), NULL);
+  task_runtime.msgq.can.feedback.shoot =
+      osMessageQueueNew(2u, sizeof(CAN_t), NULL);
+  task_runtime.msgq.can.feedback.cap =
+      osMessageQueueNew(2u, sizeof(CAN_t), NULL);
+  task_runtime.msgq.can.output.chassis =
+      osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
+  task_runtime.msgq.can.output.gimbal =
+      osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
+  task_runtime.msgq.can.output.shoot =
+      osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
+  task_runtime.msgq.can.output.cap =
+      osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
+
+  /* command */
+  task_runtime.msgq.cmd.chassis =
+      osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
+  task_runtime.msgq.cmd.gimbal =
+      osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
+  task_runtime.msgq.cmd.shoot =
+      osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
+  task_runtime.msgq.cmd.ai =
+      osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
+  task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
+
+  /* atti_esti */
+  task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
+  task_runtime.msgq.cmd.raw.host =
+      osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
+
+  task_runtime.msgq.gimbal.accl =
+      osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
+  task_runtime.msgq.gimbal.eulr_imu =
+      osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
+  task_runtime.msgq.gimbal.gyro =
+      osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
+
+  task_runtime.msgq.cap_info =
+      osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
+
+  /* AI */
+  task_runtime.msgq.ai.quat =
+      osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
+
+  /* 裁判系统 */
+  task_runtime.msgq.referee.ai =
+      osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
+  task_runtime.msgq.referee.chassis =
+      osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
+  task_runtime.msgq.referee.cap =
+      osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
+  task_runtime.msgq.referee.shoot =
+      osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
+
+  /* UI */
+  task_runtime.msgq.ui.chassis =
+      osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
+  task_runtime.msgq.ui.cap =
+      osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
+  task_runtime.msgq.ui.gimbal =
+      osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
+  task_runtime.msgq.ui.shoot =
+      osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
+  task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
+
+  osKernelUnlock();
+  osThreadTerminate(osThreadGetId()); /* 结束自身 */
+}

User/task/monitor.c

@@ -0,0 +1,59 @@
+/*
+  监控任务。
+
+  监控系统运行情况，记录错误。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\adc.h"
+#include "bsp\buzzer.h"
+#include "bsp\led.h"
+#include "bsp\usb.h"
+#include "component/capacity.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 监控
+ *
+ * \param argument 未使用
+ */
+void Task_Monitor(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_MONITOR;
+
+  osDelay(TASK_INIT_DELAY_REFEREE); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  while (1) {
+#ifdef DEBUG
+    /* 记录任务所使用的的栈空间 */
+    task_runtime.stack_water_mark.monitor =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    task_runtime.status.vbat = BSP_GetBatteryVolt(); /* ADC监测电压 */
+    task_runtime.status.battery =
+        Capacity_GetBatteryRemain(task_runtime.status.vbat);
+    task_runtime.status.cpu_temp = BSP_GetTemperature();
+        
+    bool low_bat = task_runtime.status.battery < 0.2f;
+    bool high_cpu_temp =  task_runtime.status.cpu_temp > 35.0f;
+    
+    /* 电池电量少于20%时闪烁红色LED */
+    if (low_bat || high_cpu_temp) {
+      BSP_LED_Set(BSP_LED_RED, BSP_LED_TAGGLE, 1);
+    } else {
+      BSP_LED_Set(BSP_LED_RED, BSP_LED_OFF, 1);
+    }
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+}

User/task/rc.c

@@ -0,0 +1,54 @@
+/*
+  DR16接收机通信任务
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include <string.h>
+
+#include "device\dr16.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+#ifdef DEBUG
+DR16_t dr16;
+CMD_RC_t cmd_rc;
+#else
+static DR16_t dr16;
+static CMD_RC_t cmd_rc;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief dr16接收机
+ *
+ * \param argument 未使用
+ */
+void Task_RC(void *argument) {
+  (void)argument; /* 未使用，消除警告 */
+
+  DR16_Init(&dr16); /* 初始化dr16 */
+
+  while (1) {
+#ifdef DEBUG
+    /*  */
+    task_runtime.stack_water_mark.rc = osThreadGetStackSpace(osThreadGetId());
+#endif
+    /* 开启DMA */
+    DR16_StartDmaRecv(&dr16);
+
+    if (DR16_WaitDmaCplt(20)) {
+      /* 转换 */
+      DR16_ParseRC(&dr16, &cmd_rc);
+    } else {
+      /* 处理遥控器离线 */
+      DR16_HandleOffline(&dr16, &cmd_rc);
+    }
+    osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
+    osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
+  }
+}

User/task/referee.c

@@ -0,0 +1,102 @@
+/*
+  裁判系统任务。
+
+  负责裁判系统的接收和发送。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "device\referee.h"
+
+#include "bsp\usb.h"
+#include "task\user_task.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+#ifdef DEBUG
+Referee_t ref;
+Referee_UI_t ui;
+CMD_UI_t ref_cmd;
+Referee_ForCap_t for_cap;
+Referee_ForAI_t for_ai;
+Referee_ForChassis_t for_chassis;
+Referee_ForShoot_t for_shoot;
+#else
+static Referee_t ref;
+static Referee_UI_t ui;
+static CMD_UI_t ref_cmd;
+static Referee_ForCap_t for_cap;
+static Referee_ForAI_t for_ai;
+static Referee_ForChassis_t for_chassis;
+static Referee_ForShoot_t for_shoot;
+#endif
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 裁判系统
+ *
+ * \param argument 未使用
+ */
+void Task_Referee(void *argument) {
+  (void)argument;                   /* 未使用argument，消除警告 */
+  osDelay(TASK_INIT_DELAY_REFEREE); /* 延时一段时间再开启任务 */
+
+  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_REFEREE;
+
+  /* 初始化裁判系统 */
+  Referee_Init(&ref, &ui, &(task_runtime.cfg.pilot_cfg->param.screen));
+
+  uint32_t tick = osKernelGetTickCount();
+  uint32_t last_online_tick = 0;
+  while (1) {
+#ifdef DEBUG
+    task_runtime.stack_water_mark.referee =
+        osThreadGetStackSpace(osThreadGetId());
+#endif
+    /* Task body */
+
+    Referee_StartReceiving(&ref);
+    if (osThreadFlagsWait(SIGNAL_REFEREE_RAW_REDY, osFlagsWaitAll, 10) !=
+        SIGNAL_REFEREE_RAW_REDY) {
+      if (osKernelGetTickCount() - last_online_tick > 500)
+        Referee_HandleOffline(&ref);
+    } else {
+      Referee_Parse(&ref);
+      last_online_tick = osKernelGetTickCount();
+    }
+    Referee_PackCap(&for_cap, &ref);
+    Referee_PackAI(&for_ai, &ref);
+    Referee_PackShoot(&for_shoot, &ref);
+    Referee_PackChassis(&for_chassis, &ref);
+    if (osKernelGetTickCount() > delay_tick) {
+      tick += delay_tick;
+      osMessageQueueReset(task_runtime.msgq.referee.cap);
+      osMessageQueuePut(task_runtime.msgq.referee.cap, &for_cap, 0, 0);
+      osMessageQueueReset(task_runtime.msgq.referee.ai);
+      osMessageQueuePut(task_runtime.msgq.referee.ai, &for_ai, 0, 0);
+      osMessageQueueReset(task_runtime.msgq.referee.chassis);
+      osMessageQueuePut(task_runtime.msgq.referee.chassis, &for_chassis, 0, 0);
+      osMessageQueueReset(task_runtime.msgq.referee.shoot);
+      osMessageQueuePut(task_runtime.msgq.referee.shoot, &for_shoot, 0, 0);
+
+      osMessageQueueGet(task_runtime.msgq.ui.cap, &(ui.cap_ui), NULL, 0);
+      osMessageQueueGet(task_runtime.msgq.ui.chassis, &(ui.chassis_ui), NULL,
+                        0);
+      osMessageQueueGet(task_runtime.msgq.ui.gimbal, &(ui.gimbal_ui), NULL, 0);
+      osMessageQueueGet(task_runtime.msgq.ui.shoot, &(ui.shoot_ui), NULL, 0);
+      osMessageQueueGet(task_runtime.msgq.ui.cmd, &(ui.cmd_pc), NULL, 0);
+
+      Referee_UIRefresh(&ui);
+
+      while (osMessageQueueGet(task_runtime.msgq.cmd.referee, &ref_cmd, NULL,
+                               0) == osOK) {
+        Referee_PraseCmd(&ui, ref_cmd);
+      }
+
+      Referee_PackUI(&ui, &ref);
+    }
+  }
+}

User/task/user_task.c

@@ -0,0 +1,94 @@
+/*
+  保存任务属性，生成任务时使用。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task\user_task.h"
+
+/* 机器人运行时的数据 */
+Task_Runtime_t task_runtime;
+
+/* 各个任务的参数 */
+const osThreadAttr_t attr_init = {
+    .name = "init",
+    .priority = osPriorityRealtime,
+    .stack_size = 256 * 4,
+};
+
+const osThreadAttr_t attr_ai = {
+    .name = "ai",
+    .priority = osPriorityRealtime,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_atti_esti = {
+    .name = "atti_esti",
+    .priority = osPriorityRealtime,
+    .stack_size = 256 * 4,
+};
+
+const osThreadAttr_t attr_can = {
+    .name = "can",
+    .priority = osPriorityRealtime,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_cap = {
+    .name = "cap",
+    .priority = osPriorityHigh,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_cli = {
+    .name = "cli",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+
+const osThreadAttr_t attr_command = {
+    .name = "command",
+    .priority = osPriorityHigh,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_ctrl_chassis = {
+    .name = "ctrl_chassis",
+    .priority = osPriorityAboveNormal,
+    .stack_size = 256 * 4,
+};
+
+const osThreadAttr_t attr_ctrl_gimbal = {
+    .name = "ctrl_gimbal",
+    .priority = osPriorityAboveNormal,
+    .stack_size = 256 * 4,
+};
+
+const osThreadAttr_t attr_ctrl_shoot = {
+    .name = "ctrl_shoot",
+    .priority = osPriorityAboveNormal,
+    .stack_size = 256 * 4,
+};
+
+const osThreadAttr_t attr_info = {
+    .name = "info",
+    .priority = osPriorityLow,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_monitor = {
+    .name = "monitor",
+    .priority = osPriorityBelowNormal,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_rc = {
+    .name = "rc",
+    .priority = osPriorityRealtime,
+    .stack_size = 128 * 4,
+};
+
+const osThreadAttr_t attr_referee = {
+    .name = "referee",
+    .priority = osPriorityRealtime,
+    .stack_size = 512 * 4,
+};

User/task/user_task.h

@@ -0,0 +1,216 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+
+#include "FreeRTOS.h"
+#include "module\config.h"
+#include "task.h"
+
+/* Exported constants ------------------------------------------------------- */
+
+/* 所有任务都要define一个“任务运行频率”和“初始化延时” */
+#define TASK_FREQ_CTRL_CHASSIS (500u)
+#define TASK_FREQ_CTRL_GIMBAL (500u)
+#define TASK_FREQ_CTRL_SHOOT (500)
+#define TASK_FREQ_CTRL_CAP (100u)
+#define TASK_FREQ_CTRL_COMMAND (500u)
+#define TASK_FREQ_INFO (4u)
+#define TASK_FREQ_MONITOR (2u)
+#define TASK_FREQ_CAN (1000u)
+#define TASK_FREQ_AI (250u)
+#define TASK_FREQ_REFEREE (1000u)
+
+#define TASK_INIT_DELAY_INFO (500u)
+#define TASK_INIT_DELAY_MONITOR (10)
+#define TASK_INIT_DELAY_REFEREE (400u)
+#define TASK_INIT_DELAY_AI (400u)
+
+/* Exported defines --------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  /* 各任务，也可以叫做线程 */
+  struct {
+    osThreadId_t cli;
+    osThreadId_t command;
+    osThreadId_t ctrl_chassis;
+    osThreadId_t ctrl_gimbal;
+    osThreadId_t ctrl_shoot;
+    osThreadId_t info;
+    osThreadId_t monitor;
+    osThreadId_t can;
+    osThreadId_t atti_esti;
+    osThreadId_t referee;
+    osThreadId_t ai;
+    osThreadId_t rc;
+    osThreadId_t cap;
+  } thread;
+
+  struct {
+    /* 云台相关数据 */
+    struct {
+      osMessageQueueId_t accl;     /* IMU读取 */
+      osMessageQueueId_t gyro;     /* IMU读取 */
+      osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
+    } gimbal;
+
+    /* 控制指令 */
+    struct {
+      struct {
+        osMessageQueueId_t host;
+        osMessageQueueId_t rc;
+      } raw;
+
+      osMessageQueueId_t chassis;
+      osMessageQueueId_t gimbal;
+      osMessageQueueId_t shoot;
+      osMessageQueueId_t ai;
+      osMessageQueueId_t referee;
+    } cmd;
+
+    /* can任务放入、读取，电机或电容的输入输出 */
+    struct {
+      struct {
+        osMessageQueueId_t chassis;
+        osMessageQueueId_t gimbal;
+        osMessageQueueId_t shoot;
+        osMessageQueueId_t cap;
+      } output;
+
+      struct {
+        osMessageQueueId_t chassis;
+        osMessageQueueId_t gimbal;
+        osMessageQueueId_t shoot;
+        osMessageQueueId_t cap;
+        osMessageQueueId_t tof;
+      } feedback;
+    } can;
+
+    struct {
+      osMessageQueueId_t quat; /* 姿态解算得到 */
+    } ai;
+
+    /* 裁判系统发送的 */
+    struct {
+      osMessageQueueId_t cap;
+      osMessageQueueId_t chassis;
+      osMessageQueueId_t ai;
+      osMessageQueueId_t shoot;
+    } referee;
+
+    osMessageQueueId_t cap_info;
+
+    struct {
+      osMessageQueueId_t chassis;
+      osMessageQueueId_t gimbal;
+      osMessageQueueId_t shoot;
+      osMessageQueueId_t cap;
+      osMessageQueueId_t cmd;
+    } ui;
+
+  } msgq;
+
+  /* 机器人状态 */
+  struct {
+    float battery;
+    float vbat;
+    float cpu_temp;
+  } status;
+
+  Config_t cfg; /* 机器人配置 */
+
+#ifdef DEBUG
+  struct {
+    UBaseType_t cli;
+    UBaseType_t command;
+    UBaseType_t ctrl_chassis;
+    UBaseType_t ctrl_gimbal;
+    UBaseType_t ctrl_shoot;
+    UBaseType_t info;
+    UBaseType_t monitor;
+    UBaseType_t can;
+    UBaseType_t atti_esti;
+    UBaseType_t referee;
+    UBaseType_t ai;
+    UBaseType_t rc;
+    UBaseType_t cap;
+  } stack_water_mark; /* stack使用 */
+
+  struct {
+    float cli;
+    float command;
+    float ctrl_chassis;
+    float ctrl_gimbal;
+    float ctrl_shoot;
+    float info;
+    float monitor;
+    float can;
+    float atti_esti;
+    float referee;
+    float ai;
+    float rc;
+    float cap;
+  } freq; /* 任务运行频率 */
+
+  struct {
+    float cli;
+    float command;
+    float ctrl_chassis;
+    float ctrl_gimbal;
+    float ctrl_shoot;
+    float info;
+    float monitor;
+    float can;
+    float atti_esti;
+    float referee;
+    float ai;
+    float rc;
+    float cap;
+  } last_up_time; /* 任务最近运行时间 */
+#endif
+} Task_Runtime_t;
+
+extern Task_Runtime_t task_runtime;
+
+extern const osThreadAttr_t attr_init;
+
+extern const osThreadAttr_t attr_cli;
+extern const osThreadAttr_t attr_command;
+extern const osThreadAttr_t attr_ctrl_chassis;
+extern const osThreadAttr_t attr_ctrl_gimbal;
+extern const osThreadAttr_t attr_ctrl_shoot;
+extern const osThreadAttr_t attr_info;
+extern const osThreadAttr_t attr_monitor;
+extern const osThreadAttr_t attr_can;
+extern const osThreadAttr_t attr_atti_esti;
+extern const osThreadAttr_t attr_referee;
+extern const osThreadAttr_t attr_ai;
+extern const osThreadAttr_t attr_rc;
+extern const osThreadAttr_t attr_cap;
+
+/* Exported functions prototypes -------------------------------------------- */
+void Task_Init(void *argument);
+
+void Task_CLI(void *argument);
+void Task_Command(void *argument);
+void Task_CtrlChassis(void *argument);
+void Task_CtrlGimbal(void *argument);
+void Task_CtrlShoot(void *argument);
+void Task_Info(void *argument);
+void Task_Monitor(void *argument);
+void Task_Can(void *argument);
+void Task_AttiEsti(void *argument);
+void Task_Referee(void *argument);
+void Task_Ai(void *argument);
+void Task_RC(void *argument);
+void Task_Cap(void *argument);
+
+#ifdef __cplusplus
+}
+#endif