添加ai

加ai
添加crc
2025-10-07 21:19:04 +08:00 · 2025-10-07 20:10:56 +08:00 · 2025-10-07 19:45:47 +08:00 · 2025-10-07 16:12:17 +08:00 · 2025-10-07 16:10:13 +08:00
28 changed files with 876 additions and 41 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -58,17 +58,22 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
    # User/component sources
    User/component/ahrs.c
    User/component/cmd.c
+    User/component/crc16.c
+    User/component/crc8.c
    User/component/filter.c
    User/component/kinematics.c
    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

    # User/device sources
+    User/device/ai.c
    User/device/bmi088.c
    User/device/buzzer.c
+    User/device/dm_imu.c
    User/device/dr16.c
    User/device/motor.c
    User/device/motor_dm.c
@ -87,6 +92,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
--- a/Core/Src/stm32f4xx_it.c
+++ b/Core/Src/stm32f4xx_it.c
@ -335,6 +335,7 @@ void USART1_IRQHandler(void)
  /* USER CODE END USART1_IRQn 0 */
  HAL_UART_IRQHandler(&huart1);
  /* USER CODE BEGIN USART1_IRQn 1 */
+  BSP_UART_IRQHandler(&huart1);

  /* USER CODE END USART1_IRQn 1 */
 }
--- a/User/bsp/bsp_config.yaml
+++ b/User/bsp/bsp_config.yaml
@ -129,4 +129,8 @@ uart:
  devices:
  - instance: USART3
    name: DR16
+  - instance: USART6
+    name: AI
+  - instance: USART1
+    name: REF
  enabled: true
--- a/User/bsp/uart.c
+++ b/User/bsp/uart.c
@ -25,6 +25,10 @@ static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
 static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
  if (huart->Instance == USART3)
    return BSP_UART_DR16;
+  else if (huart->Instance == USART6)
+    return BSP_UART_AI;
+  else if (huart->Instance == USART1)
+    return BSP_UART_REF;
  else
    return BSP_UART_ERR;
 }
@ -115,6 +119,10 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
  switch (uart) {
    case BSP_UART_DR16:
      return &huart3;
+    case BSP_UART_AI:
+      return &huart6;
+    case BSP_UART_REF:
+      return &huart1;
    default:
      return NULL;
  }
--- a/User/bsp/uart.h
+++ b/User/bsp/uart.h
@ -28,6 +28,8 @@ extern "C" {
 /* UART实体枚举，与设备对应 */
 typedef enum {
  BSP_UART_DR16,
+  BSP_UART_AI,
+  BSP_UART_REF,
  BSP_UART_NUM,
  BSP_UART_ERR,
 } BSP_UART_t;
--- a/User/component/component_config.yaml
+++ b/User/component/component_config.yaml
@ -6,6 +6,12 @@ cmd:
  dependencies:
  - component/ahrs
  enabled: true
+crc16:
+  dependencies: []
+  enabled: true
+crc8:
+  dependencies: []
+  enabled: true
 filter:
  dependencies:
  - component/ahrs
--- a/User/component/crc16.c
+++ b/User/component/crc16.c
@ -0,0 +1,62 @@
+#include "crc16.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+static const uint16_t crc16_tab[256] = {
+    0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
+    0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
+    0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
+    0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+    0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
+    0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
+    0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
+    0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+    0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
+    0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
+    0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
+    0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+    0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
+    0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
+    0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
+    0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+    0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
+    0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
+    0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
+    0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+    0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
+    0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
+    0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
+    0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+    0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
+    0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
+    0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
+    0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+    0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
+
+static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
+  return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
+}
+
+uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
+  while (len--) crc = CRC16_Byte(crc, *buf++);
+  return crc;
+}
+
+bool CRC16_Verify(const uint8_t *buf, size_t len) {
+  if (len < 2) return false;
+
+  uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
+  return expected ==
+         ((const uint16_t *)((const uint8_t *)buf +
+                             (len % 2)))[len / sizeof(uint16_t) - 1];
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
--- a/User/component/crc16.h
+++ b/User/component/crc16.h
@ -0,0 +1,30 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define CRC16_INIT 0XFFFF
+
+uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
+bool CRC16_Verify(const uint8_t *buf, size_t len);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif
--- a/User/component/crc8.c
+++ b/User/component/crc8.c
@ -0,0 +1,52 @@
+#include "crc8.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+static const uint8_t crc8_tab[256] = {
+    0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
+    0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
+    0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
+    0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
+    0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
+    0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
+    0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
+    0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
+    0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
+    0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
+    0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
+    0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
+    0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
+    0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
+    0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
+    0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
+    0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
+    0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
+    0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
+    0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
+    0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
+    0xd7, 0x89, 0x6b, 0x35,
+};
+
+uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
+  /* loop over the buffer data */
+  while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
+
+  return crc;
+}
+
+bool CRC8_Verify(const uint8_t *buf, size_t len) {
+  if (len < 2) return false;
+
+  uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
+  return expected == buf[len - sizeof(uint8_t)];
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
--- a/User/component/crc8.h
+++ b/User/component/crc8.h
@ -0,0 +1,30 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define CRC8_INIT 0xFF
+
+uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
+bool CRC8_Verify(const uint8_t *buf, size_t len);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif
--- a/User/component/user_math.c
+++ b/User/component/user_math.c
@ -48,13 +48,11 @@ inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
 inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }

 /**
- * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
- * 例如编码器：相差1.5PI其实等于相差-0.5PI
- *
- * \param sp 被操作的值
- * \param fb 变化量
+ * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
+ * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
+ * \param sp 设定值
+ * \param fb 反馈值
 * \param range 被操作的值变化范围，正数时起效
- *
 * \return 函数运行结果
 */
 inline float CircleError(float sp, float fb, float range) {
@ -71,9 +69,7 @@ inline float CircleError(float sp, float fb, float range) {
 }

 /**
- * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
- * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
- *
+ * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
 * \param origin 被操作的值
 * \param delta 变化量
 * \param range 被操作的值变化范围，正数时起效
--- a/User/component/user_math.h
+++ b/User/component/user_math.h
@ -21,12 +21,14 @@ extern "C" {
 #define M_DEG2RAD_MULT (0.01745329251f)
 #define M_RAD2DEG_MULT (57.2957795131f)

+#ifndef M_PI_2
+#define M_PI_2 1.57079632679f
+#endif
+
 #ifndef M_PI
 #define M_PI 3.14159265358979323846f
 #endif
-#ifndef M_PI_2
-#define M_PI_2		1.57079632679489661923f
-#endif
+
 #ifndef M_2PI
 #define M_2PI 6.28318530717958647692f
 #endif
@ -84,21 +86,17 @@ float Sign(float in);
 void ResetMoveVector(MoveVector_t *mv);

 /**
- * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
- * 例如编码器：相差1.5PI其实等于相差-0.5PI
- *
- * \param sp 被操作的值
- * \param fb 变化量
+ * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
+ * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
+ * \param sp 设定值
+ * \param fb 反馈值
 * \param range 被操作的值变化范围，正数时起效
- *
 * \return 函数运行结果
 */
 float CircleError(float sp, float fb, float range);

 /**
- * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
- * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
- *
+ * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
 * \param origin 被操作的值
 * \param delta 变化量
 * \param range 被操作的值变化范围，正数时起效
--- a/User/device/ai.c
+++ b/User/device/ai.c
@ -0,0 +1,106 @@
+/*
+AI
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "ai.h"
+
+#include <string.h>
+
+#include "bsp/time.h"
+#include "bsp/uart.h"
+#include "component/crc16.h"
+#include "component/crc8.h"
+#include "component/user_math.h"
+#include "component/filter.h"
+
+
+/* Private define ----------------------------------------------------------- */
+#define AI_LEN_RX_BUFF (sizeof(AI_DownPackage_t))
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
+
+static bool inited = false;
+
+static osThreadId_t thread_alert;
+
+static uint32_t drop_message = 0;
+
+/* Private function  -------------------------------------------------------- */
+
+static void Ai_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+static void Ai_IdleLineCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t AI_Init(AI_t *ai) {
+  UNUSED(ai);
+  if (inited) return DEVICE_ERR_INITED;
+  thread_alert = osThreadGetId();
+
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
+                            Ai_RxCpltCallback);
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
+                            Ai_IdleLineCallback);
+  
+  inited = true;
+  return 0;
+}
+
+int8_t AI_Restart(AI_t *ai) {
+  UNUSED(ai);
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  return DEVICE_OK;
+}
+
+int8_t AI_StartReceiving(AI_t *ai) {
+  UNUSED(ai);
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
+                           AI_LEN_RX_BUFF) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool AI_WaitDmaCplt(uint32_t timeout) {
+  return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, timeout) ==
+          SIGNAL_AI_RAW_REDY);
+}
+
+int8_t AI_ParseHost(AI_t *ai) {
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
+    goto error;
+  ai->header.online = true;
+  ai->header.last_online_time = BSP_TIME_Get();
+  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
+  return DEVICE_OK;
+
+error:
+  drop_message++;
+  return DEVICE_ERR;
+}
+
+// int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data){
+//   if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
+//   ai->to_host.mcu.id = AI_ID_MCU;
+//   ai->to_host.mcu.package = *data;
+//   ai->to_host.mcu.crc16 =
+//       CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2);
+//   return DEVICE_OK;
+// }
+
+int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
+
+int8_t AI_HandleOffline(AI_t *ai);
+
+int8_t AI_StartSend(AI_t *ai, bool ref_online);
+
--- a/User/device/ai.h
+++ b/User/device/ai.h
@ -0,0 +1,100 @@
+/*
+  AI
+*/
+
+#pragma once
+
+#include <sys/cdefs.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "component/ahrs.h"
+#include "component/filter.h"
+#include "component/user_math.h"
+#include "device/device.h"
+#include <cmsis_os2.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+#define AI_ID_MCU (0xC4)
+#define AI_ID_REF (0xA8)
+#define AI_ID_AI (0xA1)
+/* Exported types ----------------------------------------------------------- */
+
+/* 电控 -> 视觉 MCU数据结构体*/
+typedef struct __packed {
+  AHRS_Quaternion_t quat; /* 四元数 */
+  uint8_t notice;         /* 控制命令 */
+} AI_Protucol_UpDataMCU_t;
+
+/* 电控 -> 视觉 裁判系统数据结构体*/
+typedef struct __packed {
+  uint16_t team; /* 本身队伍 */
+  uint16_t time; /* 比赛开始时间 */
+} AI_Protocol_UpDataReferee_t;
+
+/* 电控 -> 视觉 数据包结构体*/
+typedef struct __packed {
+  AHRS_Eulr_t eulr;      /* 欧拉角 */
+  MoveVector_t move_vec; /* 运动向量 */
+  uint8_t notice;        /* 控制命令 */
+} AI_Protocol_DownData_t;
+
+/* 电控 -> 视觉 裁判系统数据包 */
+typedef struct __packed {
+  uint8_t id;                          /* 包ID */
+  AI_Protocol_UpDataReferee_t package; /* 数据包 */
+  uint16_t crc16;                      /* CRC16校验 */
+} AI_UpPackageReferee_t;
+
+/* 电控 -> 视觉 MUC数据包 */
+typedef struct __packed {
+  uint8_t id;
+  AI_Protucol_UpDataMCU_t package;
+  uint16_t crc16;
+} AI_UpPackageMCU_t;
+
+/* 视觉 -> 电控 数据包 */
+typedef struct __packed {
+  uint8_t id;                     /* 包ID */
+  AI_Protocol_DownData_t package; /* 数据包 */
+  uint16_t crc16;                 /* CRC16校验 */
+} AI_DownPackage_t;
+
+typedef struct __packed {
+  DEVICE_Header_t header; /* 设备通用头部 */
+  AI_DownPackage_t from_host;
+  struct {
+    AI_UpPackageReferee_t ref;
+    AI_UpPackageMCU_t mcu;
+  } to_host;
+} AI_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+int8_t AI_Init(AI_t *ai);
+
+int8_t AI_Restart(AI_t *ai);
+
+int8_t AI_StartReceiving(AI_t *ai);
+
+bool AI_WaitDmaCplt(uint32_t timeout);
+
+int8_t AI_ParseHost(AI_t *ai);
+
+int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data);
+
+int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
+
+int8_t AI_HandleOffline(AI_t *ai);
+
+int8_t AI_StartSend(AI_t *ai, bool ref_online);
+
+
+#ifdef __cplusplus
+}
+#endif
--- a/User/device/device.h
+++ b/User/device/device.h
@ -30,7 +30,7 @@ extern "C" {
 /* AUTO GENERATED SIGNALS END */

 /* USER SIGNALS BEGIN */
-
+#define SIGNAL_AI_RAW_REDY (1u << 5)
 /* USER SIGNALS END */
 /*设备层通用Header*/
 typedef struct {
--- a/User/device/dm_imu.c
+++ b/User/device/dm_imu.c
@ -0,0 +1,271 @@
+/*
+    DM_IMU数据获取（CAN）
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dm_imu.h"
+
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+#include <string.h>
+
+/* Private define ----------------------------------------------------------- */
+#define DM_IMU_OFFLINE_TIMEOUT      1000    // 设备离线判定时间1000ms
+
+#define ACCEL_CAN_MAX (58.8f)
+#define ACCEL_CAN_MIN	(-58.8f)
+#define GYRO_CAN_MAX	(34.88f)
+#define GYRO_CAN_MIN	(-34.88f)
+#define PITCH_CAN_MAX	(90.0f)
+#define PITCH_CAN_MIN	(-90.0f)
+#define ROLL_CAN_MAX	(180.0f)
+#define ROLL_CAN_MIN	(-180.0f)
+#define YAW_CAN_MAX		(180.0f)
+#define YAW_CAN_MIN 	(-180.0f)
+#define TEMP_MIN			(0.0f)
+#define TEMP_MAX			(60.0f)
+#define Quaternion_MIN	(-1.0f)
+#define Quaternion_MAX	(1.0f)
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+
+static uint8_t count = 0; // 计数器，用于判断设备是否离线
+/**
+ * @brief: 无符号整数转换为浮点数函数
+ */
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+
+/**
+ * @brief 解析加速度计数据
+ */
+static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int8_t temp = data[1];
+    uint16_t acc_x_raw = (data[3] << 8) | data[2];
+    uint16_t acc_y_raw = (data[5] << 8) | data[4];
+    uint16_t acc_z_raw = (data[7] << 8) | data[6];
+    imu->data.temp = (float)temp;
+    imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析陀螺仪数据
+ */
+static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t gyro_x_raw = (data[3] << 8) | data[2];
+    uint16_t gyro_y_raw = (data[5] << 8) | data[4];
+    uint16_t gyro_z_raw = (data[7] << 8) | data[6];
+    imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析欧拉角数据
+ */
+static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t pit_raw = (data[3] << 8) | data[2];
+    uint16_t yaw_raw = (data[5] << 8) | data[4];
+    uint16_t rol_raw = (data[7] << 8) | data[6];
+    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 解析四元数数据
+ */
+static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
+    int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
+    int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
+    int z = ((data[6] & 0x3F) << 8) | data[7];
+    imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
+    return DEVICE_OK;
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
+    if (imu == NULL || param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 初始化设备头部
+    imu->header.online = false;
+    imu->header.last_online_time = 0;
+    
+    // 配置参数
+    imu->param.can = param->can;
+    imu->param.can_id = param->can_id;
+    imu->param.device_id = param->device_id;
+    imu->param.master_id = param->master_id;
+    
+    // 清零数据
+    memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
+    
+    // 注册CAN接收队列，用于接收回复报文
+    int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
+    if (result != BSP_OK) {
+        return DEVICE_ERR;
+    }
+
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 请求IMU数据
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
+    uint8_t tx_data[4] = {
+        imu->param.device_id & 0xFF,        // id_L
+        (imu->param.device_id >> 8) & 0xFF, // id_H
+        (uint8_t)rid,                       // RID
+        0xCC                                // 固定值
+    };
+    
+    // 发送标准数据帧
+    BSP_CAN_StdDataFrame_t frame = {
+        .id = imu->param.can_id,
+        .dlc = 4,
+    };
+    memcpy(frame.data, tx_data, 4);
+    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
+    return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_Message_t msg;
+    int8_t result;
+    bool data_received = false;
+    
+    // 持续接收所有可用消息
+    while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
+        // 验证回复数据格式（至少检查数据长度）
+        if (msg.dlc < 3) {
+            continue; // 跳过无效消息
+        }
+        
+        // 根据数据位的第0位确定反馈报文类型
+        uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
+        
+        // 根据RID类型解析数据
+        int8_t parse_result = DEVICE_ERR;
+        switch (rid) {
+            case 0x01: // RID_ACCL
+                parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
+                break;
+            case 0x02: // RID_GYRO
+                parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
+                break;
+            case 0x03: // RID_EULER
+                parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
+                break;
+            case 0x04: // RID_QUATERNION
+                parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
+                break;
+            default:
+                continue; // 跳过未知类型的消息
+        }
+        
+        // 如果解析成功，标记为收到数据
+        if (parse_result == DEVICE_OK) {
+            data_received = true;
+        }
+    }
+    
+    // 如果收到任何有效数据，更新设备状态
+    if (data_received) {
+        imu->header.online = true;
+        imu->header.last_online_time = BSP_TIME_Get_ms();
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/** 
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
+ */ 
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    switch (count) {
+        case 0:
+            DM_IMU_Request(imu, RID_ACCL);
+            break;
+        case 1:
+            DM_IMU_Request(imu, RID_GYRO);
+            break;
+        case 2:
+            DM_IMU_Request(imu, RID_EULER);
+            break;
+        case 3:
+            DM_IMU_Request(imu, RID_QUATERNION);
+            DM_IMU_Update(imu); // 更新所有数据
+            break;
+    }
+    count++;
+    if (count >= 4) {
+        count = 0; // 重置计数器
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR;
+}
+
+/**
+ * @brief 检查设备是否在线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return false;
+    }
+    
+    uint32_t current_time = BSP_TIME_Get_ms();
+    return imu->header.online && 
+           (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
+}
--- a/User/device/dm_imu.h
+++ b/User/device/dm_imu.h
@ -0,0 +1,90 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "component/ahrs.h"
+#include "bsp/can.h"
+/* Exported constants ------------------------------------------------------- */
+
+#define DM_IMU_CAN_ID_DEFAULT 0x6FF
+#define DM_IMU_ID_DEFAULT 0x42
+#define DM_IMU_MST_ID_DEFAULT 0x43
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  BSP_CAN_t can;       // CAN总线句柄
+  uint16_t can_id;     // CAN通信ID
+  uint8_t device_id;   // 设备ID
+  uint8_t master_id;   // 主机ID
+} DM_IMU_Param_t;
+typedef enum {
+  RID_ACCL = 0x01, // 加速度计
+  RID_GYRO = 0x02, // 陀螺仪
+  RID_EULER = 0x03, // 欧拉角
+  RID_QUATERNION = 0x04, // 四元数
+} DM_IMU_RID_t;
+
+typedef struct {
+  AHRS_Accl_t accl; // 加速度计
+  AHRS_Gyro_t gyro; // 陀螺仪
+  AHRS_Eulr_t euler; // 欧拉角
+  AHRS_Quaternion_t quat; // 四元数
+  float temp; // 温度
+} DM_IMU_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DM_IMU_Param_t param; // IMU参数配置
+  DM_IMU_Data_t data; // IMU数据
+} DM_IMU_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ * @param imu DM IMU设备结构体指针
+ * @param param IMU参数配置指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
+
+/**
+ * @brief 请求IMU数据
+ * @param imu DM IMU设备结构体指针
+ * @param rid 请求的数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
+
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu);
+
+/**
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
+
+/**
+ * @brief 检查设备是否在线
+ * @param imu DM IMU设备结构体指针
+ * @return true 在线，false 离线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu);
+
+
+#ifdef __cplusplus
+}
+#endif
--- a/User/device/motor_lz.h
+++ b/User/device/motor_lz.h
@ -111,12 +111,6 @@ typedef struct {
 */
 int8_t MOTOR_LZ_Init(void);

-/**
- * @brief 反初始化灵足电机驱动系统
- * @return 设备状态码
- */
-int8_t MOTOR_LZ_DeInit(void);
-
 /**
 * @brief 注册一个灵足电机
 * @param param 电机参数
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@ -284,6 +284,8 @@ void Chassis_Output(Chassis_t *c) {
      LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
  Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
                                   c->output.wheel[1]);
+  // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
+  //                                  0.0f); // 暂时不让轮子动
 }

 int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
@ -385,7 +387,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
                         c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
  // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
-  Delta_Tp[1] = -Delta_Tp[0];
+  Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
+                PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
+                         c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);

  // 左腿控制
  {
@ -397,11 +401,13 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    // 横滚角补偿力（左腿减少支撑力）
    virtual_force[0] =
        (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
-        pid_output + 50.0f + roll_compensation_force;
+        pid_output + 60.0f + roll_compensation_force;

    // VMC逆解算（包含摆角补偿）
    if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
                         c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
+    // if (VMC_InverseSolve(&c->vmc_[0], 0.0f,
+    //                      Delta_Tp[0]) == 0) {
      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
                          &c->output.joint[1]);
    } else {
@ -421,11 +427,13 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    // 横滚角补偿力（右腿增加支撑力）
    virtual_force[1] =
        (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
-        pid_output + 55.0f - roll_compensation_force;
+        pid_output + 65.0f - roll_compensation_force;

    // VMC逆解算（包含摆角补偿）
    if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
                         c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
+    // if (VMC_InverseSolve(&c->vmc_[1], 0.0f,
+    //                      Delta_Tp[1]) == 0) {
      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
                          &c->output.joint[2]);
    } else {
--- a/User/module/config.c
+++ b/User/module/config.c
@ -100,7 +100,7 @@ Config_RobotParam_t robot_config = {
            .yaw_omega = {
                .k = 1.0f,
                .p = 1.0f,
-                .i = 0.5f,
+                .i = 0.3f,
                .d = 0.0f,
                .i_limit = 1.0f,
                .out_limit = 1.0f,
@ -108,7 +108,7 @@ Config_RobotParam_t robot_config = {
                .range = -1.0f,
            },
            .yaw_angle = {
-                .k = 10.0f,
+                .k = 8.0f,
                .p = 3.0f,
                .i = 0.0f,
                .d = 0.0f,
@ -200,8 +200,8 @@ Config_RobotParam_t robot_config = {
            .range = -1.0f,
        },
        .leg_theta={
-            .k=1.0f,
-            .p=5.0f,           /* 摆角比例系数 */
+            .k=5.0f,
+            .p=2.0f,           /* 摆角比例系数 */
            .i=0.0f,           /* 摆角积分系数 */
            .d=0.0f,           /* 摆角微分系数 */
            .i_limit=0.0f,     /* 积分限幅 */
--- 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); /* 运行结束，等待下一次唤醒 */
  }
  
 }
--- a/User/task/command.c
+++ 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); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}
--- 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
--- 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"
--- 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 */
--- 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); /* 运行结束，等待下一次唤醒 */
  }
+  
 }
--- 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,
 };
--- 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
 }
Author	SHA1	Message	Date
Robofish	ee435d8001	添加ai	2025-10-07 21:19:04 +08:00
Robofish	b8698147d5	加ai	2025-10-07 20:10:56 +08:00
Robofish	b5b1219f4a	添加crc	2025-10-07 19:45:47 +08:00
Robofish	da38e37d02	添加命令任务	2025-10-07 16:12:17 +08:00
Robofish	042ab6e390	加回rol补偿	2025-10-07 16:10:13 +08:00