mrobot好了

CMakeLists.txt

@@ -56,12 +56,14 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/bsp/uart.c
 
     # User/component sources
+    User/component/QuaternionEKF.c
     User/component/ahrs.c
     User/component/crc16.c
     User/component/crc8.c
     User/component/error_detect.c
     User/component/filter.c
     User/component/freertos_cli.c
+    User/component/kalman_filter.c
     User/component/limiter.c
     User/component/lqr.c
     User/component/pid.c
@@ -79,6 +81,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/motor_lz.c
     User/device/motor_rm.c
     User/device/vision_bridge.c
+    User/device/vofa.c
+    User/device/mrobot.c
 
     # User/module sources
     User/module/balance_chassis.c
@@ -96,7 +100,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/task/init.c
     User/task/monitor.c
     User/task/rc.c
+    User/task/cli.c
     User/task/user_task.c
+    User/task/vofa.c
 )
 
 # Add include paths
@@ -116,6 +122,7 @@ list(REMOVE_ITEM CMAKE_C_IMPLICIT_LINK_LIBRARIES ob)
 # Add linked libraries
 target_link_libraries(${CMAKE_PROJECT_NAME}
     stm32cubemx
+    ${CMAKE_SOURCE_DIR}/Drivers/CMSIS/DSP/Lib/GCC/libarm_cortexM7lfsp_math.a
 
     # Add user defined libraries
 )

Core/Inc/FreeRTOSConfig.h

@@ -51,6 +51,10 @@
 #if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__)
   #include <stdint.h>
   extern uint32_t SystemCoreClock;
+/* USER CODE BEGIN 0 */
+  extern void configureTimerForRunTimeStats(void);
+  extern unsigned long getRunTimeCounterValue(void);
+/* USER CODE END 0 */
 #endif
 #ifndef CMSIS_device_header
 #define CMSIS_device_header "stm32h7xx.h"
@@ -68,9 +72,11 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)0x10000)
+#define configTOTAL_HEAP_SIZE                    ((size_t)0x20000)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
+#define configGENERATE_RUN_TIME_STATS            1
 #define configUSE_TRACE_FACILITY                 1
+#define configUSE_STATS_FORMATTING_FUNCTIONS     1
 #define configUSE_16_BIT_TICKS                   0
 #define configUSE_MUTEXES                        1
 #define configQUEUE_REGISTRY_SIZE                8
@@ -163,6 +169,12 @@ standard names. */
 
 #define USE_CUSTOM_SYSTICK_HANDLER_IMPLEMENTATION 0
 
+/* USER CODE BEGIN 2 */
+/* Definitions needed when configGENERATE_RUN_TIME_STATS is on */
+#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS configureTimerForRunTimeStats
+#define portGET_RUN_TIME_COUNTER_VALUE getRunTimeCounterValue
+/* USER CODE END 2 */
+
 /* USER CODE BEGIN Defines */
 /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
 /* USER CODE END Defines */

Core/Inc/adc.h

@@ -34,11 +34,14 @@ extern "C" {
 
 extern ADC_HandleTypeDef hadc1;
 
+extern ADC_HandleTypeDef hadc3;
+
 /* USER CODE BEGIN Private defines */
 
 /* USER CODE END Private defines */
 
 void MX_ADC1_Init(void);
+void MX_ADC3_Init(void);
 
 /* USER CODE BEGIN Prototypes */
 

Core/Inc/bdma.h

@@ -0,0 +1,52 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    bdma.h
+  * @brief   This file contains all the function prototypes for
+  *          the bdma.c file
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2026 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __BDMA_H__
+#define __BDMA_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* DMA memory to memory transfer handles -------------------------------------*/
+
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+/* USER CODE BEGIN Private defines */
+
+/* USER CODE END Private defines */
+
+void MX_BDMA_Init(void);
+
+/* USER CODE BEGIN Prototypes */
+
+/* USER CODE END Prototypes */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BDMA_H__ */
+

Core/Inc/stm32h7xx_it.h

@@ -56,6 +56,8 @@ void DMA1_Stream0_IRQHandler(void);
 void DMA1_Stream1_IRQHandler(void);
 void DMA1_Stream2_IRQHandler(void);
 void DMA1_Stream3_IRQHandler(void);
+void DMA1_Stream4_IRQHandler(void);
+void DMA1_Stream5_IRQHandler(void);
 void FDCAN1_IT0_IRQHandler(void);
 void FDCAN2_IT0_IRQHandler(void);
 void FDCAN1_IT1_IRQHandler(void);
@@ -67,6 +69,8 @@ void USART3_IRQHandler(void);
 void EXTI15_10_IRQHandler(void);
 void UART5_IRQHandler(void);
 void UART7_IRQHandler(void);
+void ADC3_IRQHandler(void);
+void BDMA_Channel0_IRQHandler(void);
 void USART10_IRQHandler(void);
 void FDCAN3_IT0_IRQHandler(void);
 void FDCAN3_IT1_IRQHandler(void);

Core/Src/adc.c

@@ -25,7 +25,9 @@
 /* USER CODE END 0 */
 
 ADC_HandleTypeDef hadc1;
+ADC_HandleTypeDef hadc3;
 DMA_HandleTypeDef hdma_adc1;
+DMA_HandleTypeDef hdma_adc3;
 
 /* ADC1 init function */
 void MX_ADC1_Init(void)
@@ -99,36 +101,75 @@ void MX_ADC1_Init(void)
 
   /* USER CODE END ADC1_Init 2 */
 
+}
+/* ADC3 init function */
+void MX_ADC3_Init(void)
+{
+
+  /* USER CODE BEGIN ADC3_Init 0 */
+
+  /* USER CODE END ADC3_Init 0 */
+
+  ADC_ChannelConfTypeDef sConfig = {0};
+
+  /* USER CODE BEGIN ADC3_Init 1 */
+
+  /* USER CODE END ADC3_Init 1 */
+
+  /** Common config
+  */
+  hadc3.Instance = ADC3;
+  hadc3.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
+  hadc3.Init.Resolution = ADC_RESOLUTION_12B;
+  hadc3.Init.DataAlign = ADC3_DATAALIGN_RIGHT;
+  hadc3.Init.ScanConvMode = ADC_SCAN_DISABLE;
+  hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc3.Init.LowPowerAutoWait = DISABLE;
+  hadc3.Init.ContinuousConvMode = DISABLE;
+  hadc3.Init.NbrOfConversion = 1;
+  hadc3.Init.DiscontinuousConvMode = DISABLE;
+  hadc3.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+  hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc3.Init.DMAContinuousRequests = DISABLE;
+  hadc3.Init.SamplingMode = ADC_SAMPLING_MODE_NORMAL;
+  hadc3.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DR;
+  hadc3.Init.Overrun = ADC_OVR_DATA_PRESERVED;
+  hadc3.Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE;
+  hadc3.Init.OversamplingMode = DISABLE;
+  hadc3.Init.Oversampling.Ratio = ADC3_OVERSAMPLING_RATIO_2;
+  if (HAL_ADC_Init(&hadc3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Configure Regular Channel
+  */
+  sConfig.Channel = ADC_CHANNEL_VBAT;
+  sConfig.Rank = ADC_REGULAR_RANK_1;
+  sConfig.SamplingTime = ADC3_SAMPLETIME_2CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  sConfig.OffsetSign = ADC3_OFFSET_SIGN_NEGATIVE;
+  if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN ADC3_Init 2 */
+
+  /* USER CODE END ADC3_Init 2 */
+
 }
 
 void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
 {
 
   GPIO_InitTypeDef GPIO_InitStruct = {0};
-  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
   if(adcHandle->Instance==ADC1)
   {
   /* USER CODE BEGIN ADC1_MspInit 0 */
 
   /* USER CODE END ADC1_MspInit 0 */
-
-  /** Initializes the peripherals clock
-  */
-    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC;
-    PeriphClkInitStruct.PLL2.PLL2M = 2;
-    PeriphClkInitStruct.PLL2.PLL2N = 16;
-    PeriphClkInitStruct.PLL2.PLL2P = 2;
-    PeriphClkInitStruct.PLL2.PLL2Q = 2;
-    PeriphClkInitStruct.PLL2.PLL2R = 2;
-    PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_3;
-    PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE;
-    PeriphClkInitStruct.PLL2.PLL2FRACN = 0;
-    PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL2;
-    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
-    {
-      Error_Handler();
-    }
-
     /* ADC1 clock enable */
     __HAL_RCC_ADC12_CLK_ENABLE();
 
@@ -171,6 +212,39 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
 
   /* USER CODE END ADC1_MspInit 1 */
   }
+  else if(adcHandle->Instance==ADC3)
+  {
+  /* USER CODE BEGIN ADC3_MspInit 0 */
+
+  /* USER CODE END ADC3_MspInit 0 */
+    /* ADC3 clock enable */
+    __HAL_RCC_ADC3_CLK_ENABLE();
+
+    /* ADC3 DMA Init */
+    /* ADC3 Init */
+    hdma_adc3.Instance = BDMA_Channel0;
+    hdma_adc3.Init.Request = BDMA_REQUEST_ADC3;
+    hdma_adc3.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    hdma_adc3.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_adc3.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_adc3.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+    hdma_adc3.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+    hdma_adc3.Init.Mode = DMA_NORMAL;
+    hdma_adc3.Init.Priority = DMA_PRIORITY_LOW;
+    if (HAL_DMA_Init(&hdma_adc3) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(adcHandle,DMA_Handle,hdma_adc3);
+
+    /* ADC3 interrupt Init */
+    HAL_NVIC_SetPriority(ADC3_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(ADC3_IRQn);
+  /* USER CODE BEGIN ADC3_MspInit 1 */
+
+  /* USER CODE END ADC3_MspInit 1 */
+  }
 }
 
 void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
@@ -198,6 +272,23 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
 
   /* USER CODE END ADC1_MspDeInit 1 */
   }
+  else if(adcHandle->Instance==ADC3)
+  {
+  /* USER CODE BEGIN ADC3_MspDeInit 0 */
+
+  /* USER CODE END ADC3_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_ADC3_CLK_DISABLE();
+
+    /* ADC3 DMA DeInit */
+    HAL_DMA_DeInit(adcHandle->DMA_Handle);
+
+    /* ADC3 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(ADC3_IRQn);
+  /* USER CODE BEGIN ADC3_MspDeInit 1 */
+
+  /* USER CODE END ADC3_MspDeInit 1 */
+  }
 }
 
 /* USER CODE BEGIN 1 */

Core/Src/bdma.c

@@ -0,0 +1,55 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    bdma.c
+  * @brief   This file provides code for the configuration
+  *          of all the requested memory to memory DMA transfers.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2026 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Includes ------------------------------------------------------------------*/
+#include "bdma.h"
+
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+/*----------------------------------------------------------------------------*/
+/* Configure DMA                                                              */
+/*----------------------------------------------------------------------------*/
+
+/* USER CODE BEGIN 1 */
+
+/* USER CODE END 1 */
+
+/**
+  * Enable DMA controller clock
+  */
+void MX_BDMA_Init(void)
+{
+
+  /* DMA controller clock enable */
+  __HAL_RCC_BDMA_CLK_ENABLE();
+
+  /* DMA interrupt init */
+  /* BDMA_Channel0_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(BDMA_Channel0_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(BDMA_Channel0_IRQn);
+
+}
+
+/* USER CODE BEGIN 2 */
+
+/* USER CODE END 2 */
+

Core/Src/dma.c

@@ -55,6 +55,12 @@ void MX_DMA_Init(void)
   /* DMA1_Stream3_IRQn interrupt configuration */
   HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
+  /* DMA1_Stream4_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
+  /* DMA1_Stream5_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
 
 }
 

Core/Src/freertos.c

@@ -65,6 +65,23 @@ void StartDefaultTask(void *argument);
 
 void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
 
+/* Hook prototypes */
+void configureTimerForRunTimeStats(void);
+unsigned long getRunTimeCounterValue(void);
+
+/* USER CODE BEGIN 1 */
+/* Functions needed when configGENERATE_RUN_TIME_STATS is on */
+__weak void configureTimerForRunTimeStats(void)
+{
+
+}
+
+__weak unsigned long getRunTimeCounterValue(void)
+{
+return 0;
+}
+/* USER CODE END 1 */
+
 /**
   * @brief  FreeRTOS initialization
   * @param  None

Core/Src/main.c

@@ -20,6 +20,7 @@
 #include "main.h"
 #include "cmsis_os.h"
 #include "adc.h"
+#include "bdma.h"
 #include "dma.h"
 #include "fdcan.h"
 #include "octospi.h"
@@ -57,6 +58,7 @@
 
 /* Private function prototypes -----------------------------------------------*/
 void SystemClock_Config(void);
+void PeriphCommonClock_Config(void);
 void MX_FREERTOS_Init(void);
 /* USER CODE BEGIN PFP */
 
@@ -90,6 +92,9 @@ int main(void)
   /* Configure the system clock */
   SystemClock_Config();
 
+  /* Configure the peripherals common clocks */
+  PeriphCommonClock_Config();
+
   /* USER CODE BEGIN SysInit */
 
   /* USER CODE END SysInit */
@@ -97,6 +102,7 @@ int main(void)
   /* Initialize all configured peripherals */
   MX_GPIO_Init();
   MX_DMA_Init();
+  MX_BDMA_Init();
   MX_ADC1_Init();
   MX_TIM12_Init();
   MX_SPI1_Init();
@@ -113,8 +119,9 @@ int main(void)
   MX_TIM1_Init();
   MX_TIM2_Init();
   MX_OCTOSPI1_Init();
-  MX_USB_OTG_HS_PCD_Init();
   MX_UART5_Init();
+  MX_ADC3_Init();
+  MX_USB_OTG_HS_PCD_Init();
   /* USER CODE BEGIN 2 */
 
   /* USER CODE END 2 */
@@ -202,6 +209,32 @@ void SystemClock_Config(void)
   HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1);
 }
 
+/**
+  * @brief Peripherals Common Clock Configuration
+  * @retval None
+  */
+void PeriphCommonClock_Config(void)
+{
+  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
+
+  /** Initializes the peripherals clock
+  */
+  PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC;
+  PeriphClkInitStruct.PLL2.PLL2M = 2;
+  PeriphClkInitStruct.PLL2.PLL2N = 16;
+  PeriphClkInitStruct.PLL2.PLL2P = 2;
+  PeriphClkInitStruct.PLL2.PLL2Q = 2;
+  PeriphClkInitStruct.PLL2.PLL2R = 2;
+  PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_3;
+  PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE;
+  PeriphClkInitStruct.PLL2.PLL2FRACN = 0;
+  PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL2;
+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
+  {
+    Error_Handler();
+  }
+}
+
 /* USER CODE BEGIN 4 */
 
 /* USER CODE END 4 */

Core/Src/stm32h7xx_it.c

@@ -22,6 +22,7 @@
 #include "stm32h7xx_it.h"
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
+#include "bsp/uart.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -56,6 +57,8 @@
 
 /* External variables --------------------------------------------------------*/
 extern DMA_HandleTypeDef hdma_adc1;
+extern DMA_HandleTypeDef hdma_adc3;
+extern ADC_HandleTypeDef hadc3;
 extern FDCAN_HandleTypeDef hfdcan1;
 extern FDCAN_HandleTypeDef hfdcan2;
 extern FDCAN_HandleTypeDef hfdcan3;
@@ -63,6 +66,8 @@ extern DMA_HandleTypeDef hdma_spi2_rx;
 extern DMA_HandleTypeDef hdma_spi2_tx;
 extern SPI_HandleTypeDef hspi2;
 extern DMA_HandleTypeDef hdma_uart5_rx;
+extern DMA_HandleTypeDef hdma_usart1_tx;
+extern DMA_HandleTypeDef hdma_usart1_rx;
 extern UART_HandleTypeDef huart5;
 extern UART_HandleTypeDef huart7;
 extern UART_HandleTypeDef huart1;
@@ -229,6 +234,34 @@ void DMA1_Stream3_IRQHandler(void)
   /* USER CODE END DMA1_Stream3_IRQn 1 */
 }
 
+/**
+  * @brief This function handles DMA1 stream4 global interrupt.
+  */
+void DMA1_Stream4_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA1_Stream4_IRQn 0 */
+
+  /* USER CODE END DMA1_Stream4_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_usart1_tx);
+  /* USER CODE BEGIN DMA1_Stream4_IRQn 1 */
+
+  /* USER CODE END DMA1_Stream4_IRQn 1 */
+}
+
+/**
+  * @brief This function handles DMA1 stream5 global interrupt.
+  */
+void DMA1_Stream5_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA1_Stream5_IRQn 0 */
+
+  /* USER CODE END DMA1_Stream5_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_usart1_rx);
+  /* USER CODE BEGIN DMA1_Stream5_IRQn 1 */
+
+  /* USER CODE END DMA1_Stream5_IRQn 1 */
+}
+
 /**
   * @brief This function handles FDCAN1 interrupt 0.
   */
@@ -309,6 +342,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 */
 }
@@ -366,6 +400,7 @@ void UART5_IRQHandler(void)
   /* USER CODE END UART5_IRQn 0 */
   HAL_UART_IRQHandler(&huart5);
   /* USER CODE BEGIN UART5_IRQn 1 */
+  BSP_UART_IRQHandler(&huart5);
 
   /* USER CODE END UART5_IRQn 1 */
 }
@@ -384,6 +419,34 @@ void UART7_IRQHandler(void)
   /* USER CODE END UART7_IRQn 1 */
 }
 
+/**
+  * @brief This function handles ADC3 global interrupt.
+  */
+void ADC3_IRQHandler(void)
+{
+  /* USER CODE BEGIN ADC3_IRQn 0 */
+
+  /* USER CODE END ADC3_IRQn 0 */
+  HAL_ADC_IRQHandler(&hadc3);
+  /* USER CODE BEGIN ADC3_IRQn 1 */
+
+  /* USER CODE END ADC3_IRQn 1 */
+}
+
+/**
+  * @brief This function handles BDMA channel0 global interrupt.
+  */
+void BDMA_Channel0_IRQHandler(void)
+{
+  /* USER CODE BEGIN BDMA_Channel0_IRQn 0 */
+
+  /* USER CODE END BDMA_Channel0_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_adc3);
+  /* USER CODE BEGIN BDMA_Channel0_IRQn 1 */
+
+  /* USER CODE END BDMA_Channel0_IRQn 1 */
+}
+
 /**
   * @brief This function handles USART10 global interrupt.
   */

Core/Src/usart.c

@@ -31,6 +31,8 @@ UART_HandleTypeDef huart2;
 UART_HandleTypeDef huart3;
 UART_HandleTypeDef huart10;
 DMA_HandleTypeDef hdma_uart5_rx;
+DMA_HandleTypeDef hdma_usart1_tx;
+DMA_HandleTypeDef hdma_usart1_rx;
 
 /* UART5 init function */
 void MX_UART5_Init(void)
@@ -431,6 +433,43 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
     GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
+    /* USART1 DMA Init */
+    /* USART1_TX Init */
+    hdma_usart1_tx.Instance = DMA1_Stream4;
+    hdma_usart1_tx.Init.Request = DMA_REQUEST_USART1_TX;
+    hdma_usart1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
+    hdma_usart1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_usart1_tx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_usart1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_usart1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_usart1_tx.Init.Mode = DMA_NORMAL;
+    hdma_usart1_tx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_usart1_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_usart1_tx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart1_tx);
+
+    /* USART1_RX Init */
+    hdma_usart1_rx.Instance = DMA1_Stream5;
+    hdma_usart1_rx.Init.Request = DMA_REQUEST_USART1_RX;
+    hdma_usart1_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    hdma_usart1_rx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_usart1_rx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_usart1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_usart1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_usart1_rx.Init.Mode = DMA_NORMAL;
+    hdma_usart1_rx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_usart1_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_usart1_rx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart1_rx);
+
     /* USART1 interrupt Init */
     HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(USART1_IRQn);
@@ -630,6 +669,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
     */
     HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);
 
+    /* USART1 DMA DeInit */
+    HAL_DMA_DeInit(uartHandle->hdmatx);
+    HAL_DMA_DeInit(uartHandle->hdmarx);
+
     /* USART1 interrupt Deinit */
     HAL_NVIC_DisableIRQ(USART1_IRQn);
   /* USER CODE BEGIN USART1_MspDeInit 1 */

CtrBoard-H7_ALL.ioc

@@ -16,6 +16,33 @@ ADC1.Rank-1\#ChannelRegularConversion=2
 ADC1.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_32CYCLES_5
 ADC1.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_32CYCLES_5
 ADC1.master=1
+ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_VBAT
+ADC3.ClockPrescaler=ADC_CLOCK_ASYNC_DIV64
+ADC3.IPParameters=Rank-0\#ChannelRegularConversion,ClockPrescaler,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,OffsetSign-0\#ChannelRegularConversion,NbrOfConversionFlag
+ADC3.NbrOfConversionFlag=1
+ADC3.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE
+ADC3.OffsetSign-0\#ChannelRegularConversion=ADC3_OFFSET_SIGN_NEGATIVE
+ADC3.Rank-0\#ChannelRegularConversion=1
+ADC3.SamplingTime-0\#ChannelRegularConversion=ADC3_SAMPLETIME_2CYCLES_5
+Bdma.ADC3.0.Direction=DMA_PERIPH_TO_MEMORY
+Bdma.ADC3.0.EventEnable=DISABLE
+Bdma.ADC3.0.Instance=BDMA_Channel0
+Bdma.ADC3.0.MemDataAlignment=DMA_MDATAALIGN_HALFWORD
+Bdma.ADC3.0.MemInc=DMA_MINC_ENABLE
+Bdma.ADC3.0.Mode=DMA_NORMAL
+Bdma.ADC3.0.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD
+Bdma.ADC3.0.PeriphInc=DMA_PINC_DISABLE
+Bdma.ADC3.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Bdma.ADC3.0.Priority=DMA_PRIORITY_LOW
+Bdma.ADC3.0.RequestNumber=1
+Bdma.ADC3.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Bdma.ADC3.0.SignalID=NONE
+Bdma.ADC3.0.SyncEnable=DISABLE
+Bdma.ADC3.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Bdma.ADC3.0.SyncRequestNumber=1
+Bdma.ADC3.0.SyncSignalID=NONE
+Bdma.Request0=ADC3
+Bdma.RequestsNb=1
 CAD.formats=
 CAD.pinconfig=
 CAD.provider=
@@ -51,7 +78,9 @@ Dma.Request0=ADC1
 Dma.Request1=SPI2_RX
 Dma.Request2=SPI2_TX
 Dma.Request3=UART5_RX
-Dma.RequestsNb=4
+Dma.Request4=USART1_TX
+Dma.Request5=USART1_RX
+Dma.RequestsNb=6
 Dma.SPI2_RX.1.Direction=DMA_PERIPH_TO_MEMORY
 Dma.SPI2_RX.1.EventEnable=DISABLE
 Dma.SPI2_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE
@@ -106,6 +135,42 @@ Dma.UART5_RX.3.SyncEnable=DISABLE
 Dma.UART5_RX.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
 Dma.UART5_RX.3.SyncRequestNumber=1
 Dma.UART5_RX.3.SyncSignalID=NONE
+Dma.USART1_RX.5.Direction=DMA_PERIPH_TO_MEMORY
+Dma.USART1_RX.5.EventEnable=DISABLE
+Dma.USART1_RX.5.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART1_RX.5.Instance=DMA1_Stream5
+Dma.USART1_RX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART1_RX.5.MemInc=DMA_MINC_ENABLE
+Dma.USART1_RX.5.Mode=DMA_NORMAL
+Dma.USART1_RX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART1_RX.5.PeriphInc=DMA_PINC_DISABLE
+Dma.USART1_RX.5.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.USART1_RX.5.Priority=DMA_PRIORITY_LOW
+Dma.USART1_RX.5.RequestNumber=1
+Dma.USART1_RX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.USART1_RX.5.SignalID=NONE
+Dma.USART1_RX.5.SyncEnable=DISABLE
+Dma.USART1_RX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.USART1_RX.5.SyncRequestNumber=1
+Dma.USART1_RX.5.SyncSignalID=NONE
+Dma.USART1_TX.4.Direction=DMA_MEMORY_TO_PERIPH
+Dma.USART1_TX.4.EventEnable=DISABLE
+Dma.USART1_TX.4.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART1_TX.4.Instance=DMA1_Stream4
+Dma.USART1_TX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART1_TX.4.MemInc=DMA_MINC_ENABLE
+Dma.USART1_TX.4.Mode=DMA_NORMAL
+Dma.USART1_TX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART1_TX.4.PeriphInc=DMA_PINC_DISABLE
+Dma.USART1_TX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.USART1_TX.4.Priority=DMA_PRIORITY_LOW
+Dma.USART1_TX.4.RequestNumber=1
+Dma.USART1_TX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.USART1_TX.4.SignalID=NONE
+Dma.USART1_TX.4.SyncEnable=DISABLE
+Dma.USART1_TX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.USART1_TX.4.SyncRequestNumber=1
+Dma.USART1_TX.4.SyncSignalID=NONE
 FDCAN1.AutoRetransmission=ENABLE
 FDCAN1.CalculateBaudRateNominal=1000000
 FDCAN1.CalculateTimeBitNominal=1000
@@ -141,9 +206,11 @@ FDCAN3.RxFifo0ElmtsNbr=32
 FDCAN3.RxFifo1ElmtsNbr=32
 FDCAN3.StdFiltersNbr=1
 FDCAN3.TxFifoQueueElmtsNbr=32
-FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE
+FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE,configUSE_STATS_FORMATTING_FUNCTIONS,configGENERATE_RUN_TIME_STATS
 FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.configGENERATE_RUN_TIME_STATS=1
 FREERTOS.configTOTAL_HEAP_SIZE=0x10000
+FREERTOS.configUSE_STATS_FORMATTING_FUNCTIONS=1
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
 KeepUserPlacement=false
@@ -152,32 +219,34 @@ MMTConfigApplied=false
 Mcu.CPN=STM32H723VGT6
 Mcu.Family=STM32H7
 Mcu.IP0=ADC1
-Mcu.IP1=CORTEX_M7
+Mcu.IP1=ADC3
-Mcu.IP10=OCTOSPI1
+Mcu.IP10=MEMORYMAP
-Mcu.IP11=RCC
+Mcu.IP11=NVIC
-Mcu.IP12=SPI1
+Mcu.IP12=OCTOSPI1
-Mcu.IP13=SPI2
+Mcu.IP13=RCC
-Mcu.IP14=SYS
+Mcu.IP14=SPI1
-Mcu.IP15=TIM1
+Mcu.IP15=SPI2
-Mcu.IP16=TIM2
+Mcu.IP16=SYS
-Mcu.IP17=TIM3
+Mcu.IP17=TIM1
-Mcu.IP18=TIM12
+Mcu.IP18=TIM2
-Mcu.IP19=UART5
+Mcu.IP19=TIM3
-Mcu.IP2=DEBUG
+Mcu.IP2=BDMA
-Mcu.IP20=UART7
+Mcu.IP20=TIM12
-Mcu.IP21=USART1
+Mcu.IP21=UART5
-Mcu.IP22=USART2
+Mcu.IP22=UART7
-Mcu.IP23=USART3
+Mcu.IP23=USART1
-Mcu.IP24=USART10
+Mcu.IP24=USART2
-Mcu.IP25=USB_OTG_HS
+Mcu.IP25=USART3
-Mcu.IP3=DMA
+Mcu.IP26=USART10
-Mcu.IP4=FDCAN1
+Mcu.IP27=USB_OTG_HS
-Mcu.IP5=FDCAN2
+Mcu.IP3=CORTEX_M7
-Mcu.IP6=FDCAN3
+Mcu.IP4=DEBUG
-Mcu.IP7=FREERTOS
+Mcu.IP5=DMA
-Mcu.IP8=MEMORYMAP
+Mcu.IP6=FDCAN1
-Mcu.IP9=NVIC
+Mcu.IP7=FDCAN2
-Mcu.IPNb=26
+Mcu.IP8=FDCAN3
+Mcu.IP9=FREERTOS
+Mcu.IPNb=28
 Mcu.Name=STM32H723VGTx
 Mcu.Package=LQFP100
 Mcu.Pin0=PE2
@@ -238,24 +307,33 @@ Mcu.Pin58=PB3(JTDO/TRACESWO)
 Mcu.Pin59=PB5
 Mcu.Pin6=PH1-OSC_OUT
 Mcu.Pin60=PB6
-Mcu.Pin61=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin61=VP_ADC3_TempSens_Input
-Mcu.Pin62=VP_OCTOSPI1_VS_octo
+Mcu.Pin62=VP_ADC3_Vref_Input
-Mcu.Pin63=VP_SYS_VS_tim23
+Mcu.Pin63=VP_ADC3_Vbat_Input
-Mcu.Pin64=VP_MEMORYMAP_VS_MEMORYMAP
+Mcu.Pin64=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin65=VP_OCTOSPI1_VS_octo
+Mcu.Pin66=VP_SYS_VS_tim23
+Mcu.Pin67=VP_MEMORYMAP_VS_MEMORYMAP
+Mcu.Pin68=VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0
 Mcu.Pin7=PC0
 Mcu.Pin8=PC1
 Mcu.Pin9=PC2_C
-Mcu.PinsNb=65
+Mcu.PinsNb=69
-Mcu.ThirdPartyNb=0
+Mcu.ThirdParty0=STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0
+Mcu.ThirdPartyNb=1
 Mcu.UserConstants=
 Mcu.UserName=STM32H723VGTx
 MxCube.Version=6.15.0
 MxDb.Version=DB.6.0.150
+NVIC.ADC3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.BDMA_Channel0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.DMA1_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@@ -509,12 +587,12 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
 ProjectManager.ProjectStructure=
 ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x2000
-ProjectManager.TargetToolchain=MDK-ARM V5.32
+ProjectManager.TargetToolchain=CMake
 ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_TIM12_Init-TIM12-false-HAL-true,6-MX_SPI1_Init-SPI1-false-HAL-true,7-MX_SPI2_Init-SPI2-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_USART1_UART_Init-USART1-false-HAL-true,10-MX_USART2_UART_Init-USART2-false-HAL-true,11-MX_USART3_UART_Init-USART3-false-HAL-true,12-MX_UART7_Init-UART7-false-HAL-true,13-MX_USART10_UART_Init-USART10-false-HAL-true,14-MX_FDCAN1_Init-FDCAN1-false-HAL-true,15-MX_FDCAN2_Init-FDCAN2-false-HAL-true,16-MX_FDCAN3_Init-FDCAN3-false-HAL-true,17-MX_TIM1_Init-TIM1-false-HAL-true,18-MX_TIM2_Init-TIM2-false-HAL-true,19-MX_OCTOSPI1_Init-OCTOSPI1-false-HAL-true,20-MX_USB_OTG_HS_PCD_Init-USB_OTG_HS-false-HAL-true,21-MX_UART5_Init-UART5-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,5-MX_BDMA_Init-BDMA-false-HAL-true,6-MX_ADC1_Init-ADC1-false-HAL-true,7-MX_TIM12_Init-TIM12-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_SPI2_Init-SPI2-false-HAL-true,10-MX_TIM3_Init-TIM3-false-HAL-true,11-MX_USART1_UART_Init-USART1-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_UART7_Init-UART7-false-HAL-true,15-MX_USART10_UART_Init-USART10-false-HAL-true,16-MX_FDCAN1_Init-FDCAN1-false-HAL-true,17-MX_FDCAN2_Init-FDCAN2-false-HAL-true,18-MX_FDCAN3_Init-FDCAN3-false-HAL-true,19-MX_TIM1_Init-TIM1-false-HAL-true,20-MX_TIM2_Init-TIM2-false-HAL-true,21-MX_OCTOSPI1_Init-OCTOSPI1-false-HAL-true,23-MX_UART5_Init-UART5-false-HAL-true,24-MX_ADC3_Init-ADC3-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
 RCC.ADCFreq_Value=96000000
 RCC.AHB12Freq_Value=240000000
 RCC.AHB4Freq_Value=240000000
@@ -641,6 +719,10 @@ SPI2.Direction=SPI_DIRECTION_2LINES
 SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity,CLKPhase
 SPI2.Mode=SPI_MODE_MASTER
 SPI2.VirtualType=VM_MASTER
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.DSPOoLibraryJjLibrary_Checked=true
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.IPParameters=LibraryCcDSPOoLibraryJjDSPOoLibrary
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.LibraryCcDSPOoLibraryJjDSPOoLibrary=true
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0_SwParameter=LibraryCcDSPOoLibraryJjDSPOoLibrary\:true;
 TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
 TIM1.IPParameters=Channel-PWM Generation3 CH3,Channel-PWM Generation1 CH1,Period,Pulse-PWM Generation1 CH1,Pulse-PWM Generation3 CH3,Prescaler
@@ -688,12 +770,20 @@ USART3.VirtualMode-Asynchronous=VM_ASYNC
 USART3.VirtualMode-Hardware\ Flow\ Control\ (RS485)=VM_ASYNC
 USB_OTG_HS.IPParameters=VirtualMode-Device_Only_FS
 USB_OTG_HS.VirtualMode-Device_Only_FS=Device_Only_FS
+VP_ADC3_TempSens_Input.Mode=IN-TempSens
+VP_ADC3_TempSens_Input.Signal=ADC3_TempSens_Input
+VP_ADC3_Vbat_Input.Mode=IN-Vbat
+VP_ADC3_Vbat_Input.Signal=ADC3_Vbat_Input
+VP_ADC3_Vref_Input.Mode=IN-Vrefint
+VP_ADC3_Vref_Input.Signal=ADC3_Vref_Input
 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
 VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg
 VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP
 VP_OCTOSPI1_VS_octo.Mode=octo_mode
 VP_OCTOSPI1_VS_octo.Signal=OCTOSPI1_VS_octo
+VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0.Mode=DSPOoLibraryJjLibrary
+VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0.Signal=STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0
 VP_SYS_VS_tim23.Mode=TIM23
 VP_SYS_VS_tim23.Signal=SYS_VS_tim23
 board=custom

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -1323,6 +1323,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>85</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\vofa.c</PathWithFileName>
+      <FilenameWithoutPath>vofa.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1333,7 +1345,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1345,7 +1357,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1357,7 +1369,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1369,7 +1381,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1389,7 +1401,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1401,7 +1413,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1413,7 +1425,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1425,7 +1437,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1437,7 +1449,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1449,7 +1461,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1461,7 +1473,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1473,7 +1485,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1485,7 +1497,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>97</FileNumber>
+      <FileNumber>98</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1497,7 +1509,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>98</FileNumber>
+      <FileNumber>99</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1509,7 +1521,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>99</FileNumber>
+      <FileNumber>100</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1519,6 +1531,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>101</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\task\vofa.c</PathWithFileName>
+      <FilenameWithoutPath>vofa.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>

MDK-ARM/CtrBoard-H7_ALL.uvprojx

@@ -839,6 +839,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\vision_bridge.c</FilePath>
             </File>
+            <File>
+              <FileName>vofa.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\vofa.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -924,6 +929,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\task\ai.c</FilePath>
             </File>
+            <File>
+              <FileName>vofa.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\vofa.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

Middlewares/Third_Party/ARM/DSP/LICENSE.txt

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "{}"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright {yyyy} {name of copyright owner}
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

User/bsp/bsp_config.yaml

@@ -124,4 +124,6 @@ uart:
   devices:
   - instance: UART5
     name: DR16
+  - instance: USART1
+    name: VOFA
   enabled: true

User/bsp/uart.c

@@ -25,6 +25,8 @@ static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
 static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
   if (huart->Instance == UART5)
     return BSP_UART_DR16;
+  else if (huart->Instance == USART1)
+    return BSP_UART_VOFA;
   else
     return BSP_UART_ERR;
 }
@@ -115,6 +117,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
   switch (uart) {
     case BSP_UART_DR16:
       return &huart5;
+    case BSP_UART_VOFA:
+      return &huart1;
     default:
       return NULL;
   }

User/bsp/uart.h

@@ -28,6 +28,7 @@ extern "C" {
 /* UART实体枚举，与设备对应 */
 typedef enum {
   BSP_UART_DR16,
+  BSP_UART_VOFA,
   BSP_UART_NUM,
   BSP_UART_ERR,
 } BSP_UART_t;

User/component/QuaternionEKF.c

@@ -0,0 +1,527 @@
+/*
+  四元数扩展卡尔曼滤波器（QEKF）
+  基于陀螺仪零偏估计和卡方检验的姿态更新算法
+
+  一阶低通滤波器传递函数:
+      1
+   ———————
+   as + 1
+*/
+
+#include "QuaternionEKF.h"
+
+#include <string.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 全局QEKF实例 */
+QEKF_INS_t QEKF_INS;
+
+/* 状态转移矩阵F初始值 */
+static const float QEKF_F_Init[36] = {
+    1, 0, 0, 0, 0, 0,
+    0, 1, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0,
+    0, 0, 0, 1, 0, 0,
+    0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 1
+};
+
+/* 协方差矩阵P初始值 */
+static float QEKF_P_Init[36] = {
+    1, 0.1, 0.1, 0.1, 0.1, 0.1,
+    0.1, 1, 0.1, 0.1, 0.1, 0.1,
+    0.1, 0.1, 1, 0.1, 0.1, 0.1,
+    0.1, 0.1, 0.1, 1, 0.1, 0.1,
+    0.1, 0.1, 0.1, 0.1, 1, 0.1,
+    0.1, 0.1, 0.1, 0.1, 0.1, 1
+};
+
+/* 用于调试的矩阵副本 */
+static float QEKF_K[18];
+static float QEKF_H[18];
+
+/* 私有函数声明 */
+static float QEKF_InvSqrt(float x);
+static void QEKF_Observe(KF_t *kf);
+static void QEKF_F_Linearization_P_Fading(KF_t *kf);
+static void QEKF_SetH(KF_t *kf);
+static void QEKF_xhatUpdate(KF_t *kf);
+
+/**
+ * @brief 四元数EKF初始化
+ *
+ * @param process_noise1 四元数过程噪声 (推荐值: 10)
+ * @param process_noise2 陀螺仪零偏过程噪声 (推荐值: 0.001)
+ * @param measure_noise 加速度计量测噪声 (推荐值: 1000000)
+ * @param lambda 渐消因子 (推荐值: 0.9996)
+ * @param lpf 低通滤波系数 (推荐值: 0)
+ */
+void QEKF_Init(float process_noise1, float process_noise2, float measure_noise,
+               float lambda, float lpf) {
+  QEKF_INS.initialized = 1;
+  QEKF_INS.Q1 = process_noise1;
+  QEKF_INS.Q2 = process_noise2;
+  QEKF_INS.R = measure_noise;
+  QEKF_INS.chi_square_threshold = 1e-8f;
+  QEKF_INS.converge_flag = 0;
+  QEKF_INS.error_count = 0;
+  QEKF_INS.update_count = 0;
+
+  if (lambda > 1) {
+    lambda = 1;
+  }
+  QEKF_INS.lambda = lambda;
+  QEKF_INS.acc_lpf_coef = lpf;
+
+  /* 初始化卡尔曼滤波器（状态维度6，控制维度0，量测维度3） */
+  KF_Init(&QEKF_INS.qekf, 6, 0, 3);
+  KF_MatInit(&QEKF_INS.chi_square, 1, 1, QEKF_INS.chi_square_data);
+
+  /* 四元数初始化为单位四元数 */
+  QEKF_INS.qekf.xhat_data[0] = 1;
+  QEKF_INS.qekf.xhat_data[1] = 0;
+  QEKF_INS.qekf.xhat_data[2] = 0;
+  QEKF_INS.qekf.xhat_data[3] = 0;
+
+  /* 自定义函数初始化，用于扩展或增加KF的基础功能 */
+  QEKF_INS.qekf.User_Func0_f = QEKF_Observe;
+  QEKF_INS.qekf.User_Func1_f = QEKF_F_Linearization_P_Fading;
+  QEKF_INS.qekf.User_Func2_f = QEKF_SetH;
+  QEKF_INS.qekf.User_Func3_f = QEKF_xhatUpdate;
+
+  /* 设定标志位，用自定义函数替换KF标准步骤中的计算增益和后验估计 */
+  QEKF_INS.qekf.skip_eq3 = TRUE;
+  QEKF_INS.qekf.skip_eq4 = TRUE;
+
+  memcpy(QEKF_INS.qekf.F_data, QEKF_F_Init, sizeof(QEKF_F_Init));
+  memcpy(QEKF_INS.qekf.P_data, QEKF_P_Init, sizeof(QEKF_P_Init));
+}
+
+/**
+ * @brief 四元数EKF更新
+ *
+ * @param gx 陀螺仪X轴角速度 (rad/s)
+ * @param gy 陀螺仪Y轴角速度 (rad/s)
+ * @param gz 陀螺仪Z轴角速度 (rad/s)
+ * @param ax 加速度计X轴加速度 (m/s²)
+ * @param ay 加速度计Y轴加速度 (m/s²)
+ * @param az 加速度计Z轴加速度 (m/s²)
+ * @param dt 更新周期 (s)
+ */
+void QEKF_Update(float gx, float gy, float gz, float ax, float ay, float az,
+                 float dt) {
+  /* 0.5*(Ohm-Ohm^bias)*deltaT，用于更新状态转移F矩阵 */
+  float halfgxdt, halfgydt, halfgzdt;
+  float accel_inv_norm;
+
+  if (!QEKF_INS.initialized) {
+    QEKF_Init(10, 0.001f, 1000000 * 10, 0.9996f * 0 + 1, 0);
+  }
+
+  /*   F矩阵，带*号的位置需要设置
+   0      1*     2*     3*     4     5
+   6*     7      8*     9*    10    11
+  12*    13*    14     15*    16    17
+  18*    19*    20*    21     22    23
+  24     25     26     27     28    29
+  30     31     32     33     34    35
+  */
+  QEKF_INS.dt = dt;
+
+  /* 去除零偏后的陀螺仪数据 */
+  QEKF_INS.gyro[0] = gx - QEKF_INS.gyro_bias[0];
+  QEKF_INS.gyro[1] = gy - QEKF_INS.gyro_bias[1];
+  QEKF_INS.gyro[2] = gz - QEKF_INS.gyro_bias[2];
+
+  /* 设置F矩阵 */
+  halfgxdt = 0.5f * QEKF_INS.gyro[0] * dt;
+  halfgydt = 0.5f * QEKF_INS.gyro[1] * dt;
+  halfgzdt = 0.5f * QEKF_INS.gyro[2] * dt;
+
+  /* 设定状态转移矩阵F的左上角4x4子矩阵，即0.5*(Ohm-Ohm^bias)*deltaT
+     右下角有一个2x2单位阵已经初始化好了
+     注意在predict步F的右上角是4x2的零矩阵，因此每次predict都用单位阵覆盖前一轮线性化后的矩阵 */
+  memcpy(QEKF_INS.qekf.F_data, QEKF_F_Init, sizeof(QEKF_F_Init));
+
+  QEKF_INS.qekf.F_data[1] = -halfgxdt;
+  QEKF_INS.qekf.F_data[2] = -halfgydt;
+  QEKF_INS.qekf.F_data[3] = -halfgzdt;
+
+  QEKF_INS.qekf.F_data[6] = halfgxdt;
+  QEKF_INS.qekf.F_data[8] = halfgzdt;
+  QEKF_INS.qekf.F_data[9] = -halfgydt;
+
+  QEKF_INS.qekf.F_data[12] = halfgydt;
+  QEKF_INS.qekf.F_data[13] = -halfgzdt;
+  QEKF_INS.qekf.F_data[15] = halfgxdt;
+
+  QEKF_INS.qekf.F_data[18] = halfgzdt;
+  QEKF_INS.qekf.F_data[19] = halfgydt;
+  QEKF_INS.qekf.F_data[20] = -halfgxdt;
+
+  /* 加速度低通滤波，平滑数据，降低撞击和异常的影响 */
+  if (QEKF_INS.update_count == 0) {
+    /* 第一次进入，初始化低通滤波 */
+    QEKF_INS.accel[0] = ax;
+    QEKF_INS.accel[1] = ay;
+    QEKF_INS.accel[2] = az;
+  }
+  QEKF_INS.accel[0] = QEKF_INS.accel[0] * QEKF_INS.acc_lpf_coef /
+                          (QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
+                      ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
+  QEKF_INS.accel[1] = QEKF_INS.accel[1] * QEKF_INS.acc_lpf_coef /
+                          (QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
+                      ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
+  QEKF_INS.accel[2] = QEKF_INS.accel[2] * QEKF_INS.acc_lpf_coef /
+                          (QEKF_INS.dt + QEKF_INS.acc_lpf_coef) +
+                      az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.acc_lpf_coef);
+
+  /* 设置量测向量z，单位化重力加速度向量 */
+  accel_inv_norm =
+      QEKF_InvSqrt(QEKF_INS.accel[0] * QEKF_INS.accel[0] +
+                   QEKF_INS.accel[1] * QEKF_INS.accel[1] +
+                   QEKF_INS.accel[2] * QEKF_INS.accel[2]);
+  for (uint8_t i = 0; i < 3; i++) {
+    QEKF_INS.qekf.measured_vector[i] = QEKF_INS.accel[i] * accel_inv_norm;
+  }
+
+  /* 获取机体状态 */
+  QEKF_INS.gyro_norm =
+      1.0f / QEKF_InvSqrt(QEKF_INS.gyro[0] * QEKF_INS.gyro[0] +
+                          QEKF_INS.gyro[1] * QEKF_INS.gyro[1] +
+                          QEKF_INS.gyro[2] * QEKF_INS.gyro[2]);
+  QEKF_INS.accl_norm = 1.0f / accel_inv_norm;
+
+  /* 如果角速度小于阈值且加速度处于设定范围内，认为运动稳定，加速度可以用于修正角速度 */
+  if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f &&
+      QEKF_INS.accl_norm < 9.8f + 0.5f) {
+    QEKF_INS.stable_flag = 1;
+  } else {
+    QEKF_INS.stable_flag = 0;
+  }
+
+  /* 设置过程噪声Q和量测噪声R矩阵 */
+  QEKF_INS.qekf.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
+  QEKF_INS.qekf.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
+  QEKF_INS.qekf.R_data[0] = QEKF_INS.R;
+  QEKF_INS.qekf.R_data[4] = QEKF_INS.R;
+  QEKF_INS.qekf.R_data[8] = QEKF_INS.R;
+
+  /* 调用卡尔曼滤波更新，注意几个User_Funcx_f的调用 */
+  KF_Update(&QEKF_INS.qekf);
+
+  /* 获取融合后的数据，包括四元数和xy零偏值 */
+  QEKF_INS.q[0] = QEKF_INS.qekf.filtered_value[0];
+  QEKF_INS.q[1] = QEKF_INS.qekf.filtered_value[1];
+  QEKF_INS.q[2] = QEKF_INS.qekf.filtered_value[2];
+  QEKF_INS.q[3] = QEKF_INS.qekf.filtered_value[3];
+  QEKF_INS.gyro_bias[0] = QEKF_INS.qekf.filtered_value[4];
+  QEKF_INS.gyro_bias[1] = QEKF_INS.qekf.filtered_value[5];
+  QEKF_INS.gyro_bias[2] = 0; /* 大部分时候z轴通天，无法观测yaw的漂移 */
+
+  /* 利用四元数反解欧拉角 */
+  QEKF_INS.yaw =
+      atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] +
+                     QEKF_INS.q[1] * QEKF_INS.q[2]),
+             2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] +
+                     QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
+  QEKF_INS.pitch =
+      atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] +
+                     QEKF_INS.q[2] * QEKF_INS.q[3]),
+             2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] +
+                     QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
+  QEKF_INS.roll =
+      asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] -
+                     QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+
+  /* 获取yaw总角度，处理超过360度的情况（如小陀螺） */
+  if (QEKF_INS.yaw - QEKF_INS.yaw_angle_last > 180.0f) {
+    QEKF_INS.yaw_round_count--;
+  } else if (QEKF_INS.yaw - QEKF_INS.yaw_angle_last < -180.0f) {
+    QEKF_INS.yaw_round_count++;
+  }
+  QEKF_INS.yaw_total_angle = 360.0f * QEKF_INS.yaw_round_count + QEKF_INS.yaw;
+  QEKF_INS.yaw_angle_last = QEKF_INS.yaw;
+  QEKF_INS.update_count++;
+}
+
+/**
+ * @brief 更新线性化后的状态转移矩阵F右上角的4x2分块矩阵，用于协方差矩阵P的更新
+ *        并对零偏的方差进行限制，防止过度收敛并限幅防止发散
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_F_Linearization_P_Fading(KF_t *kf) {
+  float q0, q1, q2, q3;
+  float q_inv_norm;
+
+  q0 = kf->xhatminus_data[0];
+  q1 = kf->xhatminus_data[1];
+  q2 = kf->xhatminus_data[2];
+  q3 = kf->xhatminus_data[3];
+
+  /* 四元数归一化 */
+  q_inv_norm = QEKF_InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  for (uint8_t i = 0; i < 4; i++) {
+    kf->xhatminus_data[i] *= q_inv_norm;
+  }
+
+  /*  F矩阵，带*号的位置需要设置
+   0     1     2     3     4*     5*
+   6     7     8     9    10*    11*
+  12    13    14    15    16*    17*
+  18    19    20    21    22*    23*
+  24    25    26    27    28     29
+  30    31    32    33    34     35
+  */
+  /* 设置F矩阵右上角4x2分块 */
+  kf->F_data[4] = q1 * QEKF_INS.dt / 2;
+  kf->F_data[5] = q2 * QEKF_INS.dt / 2;
+
+  kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
+  kf->F_data[11] = q3 * QEKF_INS.dt / 2;
+
+  kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
+  kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
+
+  kf->F_data[22] = q2 * QEKF_INS.dt / 2;
+  kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
+
+  /* 渐消滤波，防止零偏参数过度收敛 */
+  kf->P_data[28] /= QEKF_INS.lambda;
+  kf->P_data[35] /= QEKF_INS.lambda;
+
+  /* 限幅，防止发散 */
+  if (kf->P_data[28] > 10000) {
+    kf->P_data[28] = 10000;
+  }
+  if (kf->P_data[35] > 10000) {
+    kf->P_data[35] = 10000;
+  }
+}
+
+/**
+ * @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_SetH(KF_t *kf) {
+  float doubleq0, doubleq1, doubleq2, doubleq3;
+
+  /* H矩阵布局
+   0     1     2     3     4     5
+   6     7     8     9    10    11
+  12    13    14    15    16    17
+  最后两列是零
+  */
+  doubleq0 = 2 * kf->xhatminus_data[0];
+  doubleq1 = 2 * kf->xhatminus_data[1];
+  doubleq2 = 2 * kf->xhatminus_data[2];
+  doubleq3 = 2 * kf->xhatminus_data[3];
+
+  memset(kf->H_data, 0, sizeof(float) * kf->z_size * kf->xhat_size);
+
+  kf->H_data[0] = -doubleq2;
+  kf->H_data[1] = doubleq3;
+  kf->H_data[2] = -doubleq0;
+  kf->H_data[3] = doubleq1;
+
+  kf->H_data[6] = doubleq1;
+  kf->H_data[7] = doubleq0;
+  kf->H_data[8] = doubleq3;
+  kf->H_data[9] = doubleq2;
+
+  kf->H_data[12] = doubleq0;
+  kf->H_data[13] = -doubleq1;
+  kf->H_data[14] = -doubleq2;
+  kf->H_data[15] = doubleq3;
+}
+
+/**
+ * @brief 利用观测值和先验估计得到最优的后验估计
+ *        加入了卡方检验以判断融合加速度的条件是否满足
+ *        同时引入发散保护保证恶劣工况下的必要量测更新
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_xhatUpdate(KF_t *kf) {
+  float q0, q1, q2, q3;
+
+  kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
+  kf->temp_matrix.numRows = kf->H.numRows;
+  kf->temp_matrix.numCols = kf->Pminus.numCols;
+  /* temp_matrix = H·P'(k) */
+  kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
+  kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
+  kf->temp_matrix1.numCols = kf->HT.numCols;
+  /* temp_matrix1 = H·P'(k)·HT */
+  kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
+  kf->S.numRows = kf->R.numRows;
+  kf->S.numCols = kf->R.numCols;
+  /* S = H·P'(k)·HT + R */
+  kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
+  /* temp_matrix1 = inv(H·P'(k)·HT + R) */
+  kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
+
+  q0 = kf->xhatminus_data[0];
+  q1 = kf->xhatminus_data[1];
+  q2 = kf->xhatminus_data[2];
+  q3 = kf->xhatminus_data[3];
+
+  kf->temp_vector.numRows = kf->H.numRows;
+  kf->temp_vector.numCols = 1;
+  /* 计算预测得到的重力加速度方向（通过姿态获取） */
+  kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
+  kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
+  /* temp_vector = h(xhat'(k)) */
+  kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+
+  /* 计算预测值和各个轴的方向余弦 */
+  for (uint8_t i = 0; i < 3; i++) {
+    QEKF_INS.orientation_cosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
+  }
+
+  /* 利用加速度计数据修正 */
+  kf->temp_vector1.numRows = kf->z.numRows;
+  kf->temp_vector1.numCols = 1;
+  /* temp_vector1 = z(k) - h(xhat'(k)) */
+  kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
+
+  /* 卡方检验 */
+  kf->temp_matrix.numRows = kf->temp_vector1.numRows;
+  kf->temp_matrix.numCols = 1;
+  /* temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k))) */
+  kf->mat_status =
+      KF_MatMult(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix);
+  kf->temp_vector.numRows = 1;
+  kf->temp_vector.numCols = kf->temp_vector1.numRows;
+  /* temp_vector = (z(k) - h(xhat'(k)))' */
+  kf->mat_status = KF_MatTrans(&kf->temp_vector1, &kf->temp_vector);
+  kf->mat_status =
+      KF_MatMult(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.chi_square);
+
+  /* rk较小，滤波器已收敛/正在收敛 */
+  if (QEKF_INS.chi_square_data[0] < 0.5f * QEKF_INS.chi_square_threshold) {
+    QEKF_INS.converge_flag = 1;
+  }
+
+  /* rk大于阈值但曾经收敛过 */
+  if (QEKF_INS.chi_square_data[0] > QEKF_INS.chi_square_threshold &&
+      QEKF_INS.converge_flag) {
+    if (QEKF_INS.stable_flag) {
+      QEKF_INS.error_count++; /* 载体静止时仍无法通过卡方检验 */
+    } else {
+      QEKF_INS.error_count = 0;
+    }
+
+    if (QEKF_INS.error_count > 50) {
+      /* 滤波器发散 */
+      QEKF_INS.converge_flag = 0;
+      kf->skip_eq5 = FALSE; /* 步骤5是协方差矩阵P更新 */
+    } else {
+      /* 残差未通过卡方检验，仅预测 */
+      /* xhat(k) = xhat'(k) */
+      /* P(k) = P'(k) */
+      memcpy(kf->xhat_data, kf->xhatminus_data,
+             sizeof(float) * kf->xhat_size);
+      memcpy(kf->P_data, kf->Pminus_data,
+             sizeof(float) * kf->xhat_size * kf->xhat_size);
+      kf->skip_eq5 = TRUE; /* 跳过P更新 */
+      return;
+    }
+  } else {
+    /* 发散或rk不大/可接受，使用自适应增益 */
+    /* 自适应缩放，rk越小则增益越大，否则更相信预测值 */
+    if (QEKF_INS.chi_square_data[0] > 0.1f * QEKF_INS.chi_square_threshold &&
+        QEKF_INS.converge_flag) {
+      QEKF_INS.adaptive_gain_scale =
+          (QEKF_INS.chi_square_threshold - QEKF_INS.chi_square_data[0]) /
+          (0.9f * QEKF_INS.chi_square_threshold);
+    } else {
+      QEKF_INS.adaptive_gain_scale = 1;
+    }
+    QEKF_INS.error_count = 0;
+    kf->skip_eq5 = FALSE;
+  }
+
+  /* 计算卡尔曼增益K */
+  kf->temp_matrix.numRows = kf->Pminus.numRows;
+  kf->temp_matrix.numCols = kf->HT.numCols;
+  /* temp_matrix = P'(k)·HT */
+  kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
+  kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
+
+  /* 应用自适应增益 */
+  for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++) {
+    kf->K_data[i] *= QEKF_INS.adaptive_gain_scale;
+  }
+  for (uint8_t i = 4; i < 6; i++) {
+    for (uint8_t j = 0; j < 3; j++) {
+      kf->K_data[i * 3 + j] *=
+          QEKF_INS.orientation_cosine[i - 4] / 1.5707963f; /* 1 rad */
+    }
+  }
+
+  kf->temp_vector.numRows = kf->K.numRows;
+  kf->temp_vector.numCols = 1;
+  /* temp_vector = K(k)·(z(k) - H·xhat'(k)) */
+  kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
+
+  /* 零偏修正限幅，一般不会有过大的漂移 */
+  if (QEKF_INS.converge_flag) {
+    for (uint8_t i = 4; i < 6; i++) {
+      if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt) {
+        kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
+      }
+      if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt) {
+        kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
+      }
+    }
+  }
+
+  /* 不修正yaw轴数据 */
+  kf->temp_vector.pData[3] = 0;
+  kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
+}
+
+/**
+ * @brief EKF观测环节，复制数据用于调试
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void QEKF_Observe(KF_t *kf) {
+  memcpy(QEKF_P_Init, kf->P_data, sizeof(QEKF_P_Init));
+  memcpy(QEKF_K, kf->K_data, sizeof(QEKF_K));
+  memcpy(QEKF_H, kf->H_data, sizeof(QEKF_H));
+}
+
+/**
+ * @brief 快速计算1/sqrt(x)
+ *
+ * @param x 输入值
+ * @return float 1/sqrt(x)的近似值
+ */
+static float QEKF_InvSqrt(float x) {
+  float halfx = 0.5f * x;
+  float y = x;
+  long i = *(long *)&y;
+  i = 0x5f375a86 - (i >> 1);
+  y = *(float *)&i;
+  y = y * (1.5f - (halfx * y * y));
+  return y;
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

User/component/QuaternionEKF.h

@@ -0,0 +1,111 @@
+/*
+  四元数扩展卡尔曼滤波器（QEKF）
+  基于陀螺仪零偏估计和卡方检验的姿态更新算法
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "kalman_filter.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 布尔类型定义 */
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+/* 四元数EKF惯性导航结构体 */
+typedef struct {
+  uint8_t initialized;      /* 初始化标志 */
+  KF_t qekf;                /* 卡尔曼滤波器实例 */
+  uint8_t converge_flag;    /* 收敛标志 */
+  uint8_t stable_flag;      /* 稳定标志 */
+  uint64_t error_count;     /* 错误计数 */
+  uint64_t update_count;    /* 更新计数 */
+
+  float q[4];               /* 四元数估计值 */
+  float gyro_bias[3];       /* 陀螺仪零偏估计值 */
+
+  float gyro[3];            /* 陀螺仪数据 */
+  float accel[3];           /* 加速度计数据 */
+
+  float orientation_cosine[3]; /* 方向余弦 */
+
+  float acc_lpf_coef;       /* 加速度低通滤波系数 */
+  float gyro_norm;          /* 陀螺仪模值 */
+  float accl_norm;          /* 加速度计模值 */
+  float adaptive_gain_scale; /* 自适应增益缩放 */
+
+  float roll;               /* 横滚角 */
+  float pitch;              /* 俯仰角 */
+  float yaw;                /* 偏航角 */
+
+  float yaw_total_angle;    /* 偏航总角度 */
+
+  float Q1;                 /* 四元数更新过程噪声 */
+  float Q2;                 /* 陀螺仪零偏过程噪声 */
+  float R;                  /* 加速度计量测噪声 */
+
+  float dt;                 /* 姿态更新周期 */
+  KF_Mat chi_square;        /* 卡方检验矩阵 */
+  float chi_square_data[1]; /* 卡方检验数据 */
+  float chi_square_threshold; /* 卡方检验阈值 */
+  float lambda;             /* 渐消因子 */
+
+  int16_t yaw_round_count;  /* 偏航圈数计数 */
+  float yaw_angle_last;     /* 上次偏航角 */
+} QEKF_INS_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+extern QEKF_INS_t QEKF_INS;
+
+/**
+ * @brief 四元数EKF初始化
+ *
+ * @param process_noise1 四元数过程噪声 (推荐值: 10)
+ * @param process_noise2 陀螺仪零偏过程噪声 (推荐值: 0.001)
+ * @param measure_noise 加速度计量测噪声 (推荐值: 1000000)
+ * @param lambda 渐消因子 (推荐值: 0.9996)
+ * @param lpf 低通滤波系数 (推荐值: 0)
+ */
+void QEKF_Init(float process_noise1, float process_noise2, float measure_noise,
+               float lambda, float lpf);
+
+/**
+ * @brief 四元数EKF更新
+ *
+ * @param gx 陀螺仪X轴角速度 (rad/s)
+ * @param gy 陀螺仪Y轴角速度 (rad/s)
+ * @param gz 陀螺仪Z轴角速度 (rad/s)
+ * @param ax 加速度计X轴加速度 (m/s²)
+ * @param ay 加速度计Y轴加速度 (m/s²)
+ * @param az 加速度计Z轴加速度 (m/s²)
+ * @param dt 更新周期 (s)
+ */
+void QEKF_Update(float gx, float gy, float gz, float ax, float ay, float az,
+                 float dt);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

User/component/freertos_cli.h

@@ -28,6 +28,9 @@
 #ifndef COMMAND_INTERPRETER_H
 #define COMMAND_INTERPRETER_H
 
+#include <stddef.h>
+#include "FreeRTOS.h"
+
 /* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */
 #define configCOMMAND_INT_MAX_OUTPUT_SIZE  512
 

User/component/kalman_filter.c

@@ -0,0 +1,591 @@
+/*
+  卡尔曼滤波器 modified from wang hongxi
+  支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
+
+  主要特性：
+  - 基于量测有效性的 H、R、K 矩阵动态调整
+  - 支持不同传感器采样频率
+  - 矩阵 P 防过度收敛机制
+  - ARM CMSIS DSP 优化的矩阵运算
+  - 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF）
+
+  使用说明：
+  1. 矩阵初始化：P、F、Q 使用标准初始化方式
+     H、R 在使用自动调整时需要配置量测映射
+
+  2. 自动调整模式 (use_auto_adjustment = 1)：
+     - 提供 measurement_map：每个量测对应的状态索引
+     - 提供 measurement_degree：H 矩阵元素值
+     - 提供 mat_r_diagonal_elements：量测噪声方差
+
+  3. 固定模式 (use_auto_adjustment = 0)：
+     - 像初始化 P 矩阵那样正常初始化 z、H、R
+
+  4. 量测更新：
+     - 在传感器回调函数中更新 measured_vector
+     - 值为 0 表示量测无效
+     - 向量在每次 KF 更新后会被重置为 0
+
+  5. 防过度收敛：
+     - 设置 state_min_variance 防止 P 矩阵过度收敛
+     - 帮助滤波器适应缓慢变化的状态
+
+  使用示例：高度估计
+  状态向量 x =
+    |   高度      |
+    |   速度      |
+    |   加速度    |
+
+  KF_t Height_KF;
+
+  void INS_Task_Init(void)
+  {
+      // 初始协方差矩阵 P 
+      static float P_Init[9] =
+      {
+          10, 0, 0,
+          0, 30, 0,
+          0, 0, 10,
+      };
+
+      // 状态转移矩阵 F（根据运动学模型）
+      static float F_Init[9] =
+      {
+          1, dt, 0.5*dt*dt,
+          0, 1, dt,
+          0, 0, 1,
+      };
+
+      // 过程噪声协方差矩阵 Q
+      static float Q_Init[9] =
+      {
+          0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
+          0.5*dt*dt*dt,        dt*dt,         dt,
+          0.5*dt*dt,              dt,         1,
+      };
+
+      // 设置状态最小方差（防止过度收敛）
+      static float state_min_variance[3] = {0.03, 0.005, 0.1};
+
+      // 开启自动调整
+      Height_KF.use_auto_adjustment = 1;
+
+      // 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3]
+      static uint8_t measurement_reference[3] = {1, 1, 3};
+
+      // 量测系数（H矩阵元素值）
+      static float measurement_degree[3] = {1, 1, 1};
+      // 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下
+      // （在当前周期全部量测数据有效的情况下）
+      //   |1   0   0|
+      //   |1   0   0|
+      //   |0   0   1|
+
+      // 量测噪声方差（R矩阵对角元素）
+      static float mat_r_diagonal_elements[3] = {30, 25, 35};
+      // 根据 mat_r_diagonal_elements 生成 R 矩阵如下
+      // （在当前周期全部量测数据有效的情况下）
+      //   |30   0   0|
+      //   | 0  25   0|
+      //   | 0   0  35|
+
+      // 初始化卡尔曼滤波器（状态维数3，控制维数0，量测维数3）
+      KF_Init(&Height_KF, 3, 0, 3);
+
+      // 设置矩阵初值
+      memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
+      memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
+      memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
+      memcpy(Height_KF.measurement_map, measurement_reference,
+             sizeof(measurement_reference));
+      memcpy(Height_KF.measurement_degree, measurement_degree,
+             sizeof(measurement_degree));
+      memcpy(Height_KF.mat_r_diagonal_elements, mat_r_diagonal_elements,
+             sizeof(mat_r_diagonal_elements));
+      memcpy(Height_KF.state_min_variance, state_min_variance,
+             sizeof(state_min_variance));
+  }
+
+  void INS_Task(void const *pvParameters)
+  {
+      // 循环更新卡尔曼滤波器
+      KF_Update(&Height_KF);
+      vTaskDelay(ts);
+  }
+
+  // 传感器回调函数示例：在数据就绪时更新 measured_vector
+  void Barometer_Read_Over(void)
+  {
+      ......
+      INS_KF.measured_vector[0] = baro_height;  // 气压计高度
+  }
+
+  void GPS_Read_Over(void)
+  {
+      ......
+      INS_KF.measured_vector[1] = GPS_height;   // GPS高度
+  }
+
+  void Acc_Data_Process(void)
+  {
+      ......
+      INS_KF.measured_vector[2] = acc.z;        // Z轴加速度
+  }
+*/
+
+#include "kalman_filter.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 私有函数声明 */
+static void KF_AdjustHKR(KF_t *kf);
+
+/**
+ * @brief 初始化卡尔曼滤波器并分配矩阵内存
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @param xhat_size 状态向量维度
+ * @param u_size 控制向量维度（无控制输入时设为0）
+ * @param z_size 量测向量维度
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
+  if (kf == NULL) return -1;
+
+  kf->xhat_size = xhat_size;
+  kf->u_size = u_size;
+  kf->z_size = z_size;
+
+  kf->measurement_valid_num = 0;
+
+  /* 量测标志分配 */
+  kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
+  memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size);
+
+  kf->measurement_degree = (float *)user_malloc(sizeof(float) * z_size);
+  memset(kf->measurement_degree, 0, sizeof(float) * z_size);
+
+  kf->mat_r_diagonal_elements = (float *)user_malloc(sizeof(float) * z_size);
+  memset(kf->mat_r_diagonal_elements, 0, sizeof(float) * z_size);
+
+  kf->state_min_variance = (float *)user_malloc(sizeof(float) * xhat_size);
+  memset(kf->state_min_variance, 0, sizeof(float) * xhat_size);
+
+  kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
+  memset(kf->temp, 0, sizeof(uint8_t) * z_size);
+
+  /* 滤波数据分配 */
+  kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size);
+  memset(kf->filtered_value, 0, sizeof(float) * xhat_size);
+
+  kf->measured_vector = (float *)user_malloc(sizeof(float) * z_size);
+  memset(kf->measured_vector, 0, sizeof(float) * z_size);
+
+  kf->control_vector = (float *)user_malloc(sizeof(float) * u_size);
+  memset(kf->control_vector, 0, sizeof(float) * u_size);
+
+  /* 状态估计 xhat x(k|k) */
+  kf->xhat_data = (float *)user_malloc(sizeof(float) * xhat_size);
+  memset(kf->xhat_data, 0, sizeof(float) * xhat_size);
+  KF_MatInit(&kf->xhat, kf->xhat_size, 1, kf->xhat_data);
+
+  /* 先验状态估计 xhatminus x(k|k-1) */
+  kf->xhatminus_data = (float *)user_malloc(sizeof(float) * xhat_size);
+  memset(kf->xhatminus_data, 0, sizeof(float) * xhat_size);
+  KF_MatInit(&kf->xhatminus, kf->xhat_size, 1, kf->xhatminus_data);
+
+  /* 控制向量 u */
+  if (u_size != 0) {
+    kf->u_data = (float *)user_malloc(sizeof(float) * u_size);
+    memset(kf->u_data, 0, sizeof(float) * u_size);
+    KF_MatInit(&kf->u, kf->u_size, 1, kf->u_data);
+  }
+
+  /* 量测向量 z */
+  kf->z_data = (float *)user_malloc(sizeof(float) * z_size);
+  memset(kf->z_data, 0, sizeof(float) * z_size);
+  KF_MatInit(&kf->z, kf->z_size, 1, kf->z_data);
+
+  /* 协方差矩阵 P(k|k) */
+  kf->P_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  memset(kf->P_data, 0, sizeof(float) * xhat_size * xhat_size);
+  KF_MatInit(&kf->P, kf->xhat_size, kf->xhat_size, kf->P_data);
+
+  /* 先验协方差矩阵 P(k|k-1) */
+  kf->Pminus_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  memset(kf->Pminus_data, 0, sizeof(float) * xhat_size * xhat_size);
+  KF_MatInit(&kf->Pminus, kf->xhat_size, kf->xhat_size, kf->Pminus_data);
+
+  /* 状态转移矩阵 F 及其转置 FT */
+  kf->F_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  kf->FT_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  memset(kf->F_data, 0, sizeof(float) * xhat_size * xhat_size);
+  memset(kf->FT_data, 0, sizeof(float) * xhat_size * xhat_size);
+  KF_MatInit(&kf->F, kf->xhat_size, kf->xhat_size, kf->F_data);
+  KF_MatInit(&kf->FT, kf->xhat_size, kf->xhat_size, kf->FT_data);
+
+  /* 控制矩阵 B */
+  if (u_size != 0) {
+    kf->B_data = (float *)user_malloc(sizeof(float) * xhat_size * u_size);
+    memset(kf->B_data, 0, sizeof(float) * xhat_size * u_size);
+    KF_MatInit(&kf->B, kf->xhat_size, kf->u_size, kf->B_data);
+  }
+
+  /* 量测矩阵 H 及其转置 HT */
+  kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_size);
+  kf->HT_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
+  memset(kf->H_data, 0, sizeof(float) * z_size * xhat_size);
+  memset(kf->HT_data, 0, sizeof(float) * xhat_size * z_size);
+  KF_MatInit(&kf->H, kf->z_size, kf->xhat_size, kf->H_data);
+  KF_MatInit(&kf->HT, kf->xhat_size, kf->z_size, kf->HT_data);
+
+  /* 过程噪声协方差矩阵 Q */
+  kf->Q_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  memset(kf->Q_data, 0, sizeof(float) * xhat_size * xhat_size);
+  KF_MatInit(&kf->Q, kf->xhat_size, kf->xhat_size, kf->Q_data);
+
+  /* 量测噪声协方差矩阵 R */
+  kf->R_data = (float *)user_malloc(sizeof(float) * z_size * z_size);
+  memset(kf->R_data, 0, sizeof(float) * z_size * z_size);
+  KF_MatInit(&kf->R, kf->z_size, kf->z_size, kf->R_data);
+
+  /* 卡尔曼增益 K */
+  kf->K_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
+  memset(kf->K_data, 0, sizeof(float) * xhat_size * z_size);
+  KF_MatInit(&kf->K, kf->xhat_size, kf->z_size, kf->K_data);
+
+  /* 临时矩阵分配 */
+  kf->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  kf->temp_matrix_data =
+      (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  kf->temp_matrix_data1 =
+      (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
+  kf->temp_vector_data = (float *)user_malloc(sizeof(float) * xhat_size);
+  kf->temp_vector_data1 = (float *)user_malloc(sizeof(float) * xhat_size);
+
+  KF_MatInit(&kf->S, kf->xhat_size, kf->xhat_size, kf->S_data);
+  KF_MatInit(&kf->temp_matrix, kf->xhat_size, kf->xhat_size,
+             kf->temp_matrix_data);
+  KF_MatInit(&kf->temp_matrix1, kf->xhat_size, kf->xhat_size,
+             kf->temp_matrix_data1);
+  KF_MatInit(&kf->temp_vector, kf->xhat_size, 1, kf->temp_vector_data);
+  KF_MatInit(&kf->temp_vector1, kf->xhat_size, 1, kf->temp_vector_data1);
+
+  /* 初始化跳过标志 */
+  kf->skip_eq1 = 0;
+  kf->skip_eq2 = 0;
+  kf->skip_eq3 = 0;
+  kf->skip_eq4 = 0;
+  kf->skip_eq5 = 0;
+
+  /* 初始化用户函数指针 */
+  kf->User_Func0_f = NULL;
+  kf->User_Func1_f = NULL;
+  kf->User_Func2_f = NULL;
+  kf->User_Func3_f = NULL;
+  kf->User_Func4_f = NULL;
+  kf->User_Func5_f = NULL;
+  kf->User_Func6_f = NULL;
+
+  return 0;
+}
+
+/**
+ * @brief 获取量测并在启用自动调整时调整矩阵
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_Measure(KF_t *kf) {
+  if (kf == NULL) return -1;
+
+  /* 根据量测有效性自动调整 H, K, R 矩阵 */
+  if (kf->use_auto_adjustment != 0) {
+    KF_AdjustHKR(kf);
+  } else {
+    memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size);
+    memset(kf->measured_vector, 0, sizeof(float) * kf->z_size);
+  }
+
+  memcpy(kf->u_data, kf->control_vector, sizeof(float) * kf->u_size);
+
+  return 0;
+}
+
+/**
+ * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_PredictState(KF_t *kf) {
+  if (kf == NULL) return -1;
+
+  if (!kf->skip_eq1) {
+    if (kf->u_size > 0) {
+      /* 有控制输入: xhat'(k) = F·xhat(k-1) + B·u */
+      kf->temp_vector.numRows = kf->xhat_size;
+      kf->temp_vector.numCols = 1;
+      kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector);
+
+      kf->temp_vector1.numRows = kf->xhat_size;
+      kf->temp_vector1.numCols = 1;
+      kf->mat_status = KF_MatMult(&kf->B, &kf->u, &kf->temp_vector1);
+      kf->mat_status =
+          KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
+    } else {
+      /* 无控制输入: xhat'(k) = F·xhat(k-1) */
+      kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus);
+    }
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_PredictCovariance(KF_t *kf) {
+  if (kf == NULL) return -1;
+
+  if (!kf->skip_eq2) {
+    kf->mat_status = KF_MatTrans(&kf->F, &kf->FT);
+    kf->mat_status = KF_MatMult(&kf->F, &kf->P, &kf->Pminus);
+    kf->temp_matrix.numRows = kf->Pminus.numRows;
+    kf->temp_matrix.numCols = kf->FT.numCols;
+    /* F·P(k-1)·F^T */
+    kf->mat_status = KF_MatMult(&kf->Pminus, &kf->FT, &kf->temp_matrix);
+    kf->mat_status = KF_MatAdd(&kf->temp_matrix, &kf->Q, &kf->Pminus);
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_CalcGain(KF_t *kf) {
+  if (kf == NULL) return -1;
+
+  if (!kf->skip_eq3) {
+    kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
+    kf->temp_matrix.numRows = kf->H.numRows;
+    kf->temp_matrix.numCols = kf->Pminus.numCols;
+    /* H·P'(k) */
+    kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
+    kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
+    kf->temp_matrix1.numCols = kf->HT.numCols;
+    /* H·P'(k)·H^T */
+    kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
+    kf->S.numRows = kf->R.numRows;
+    kf->S.numCols = kf->R.numCols;
+    /* S = H·P'(k)·H^T + R */
+    kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
+    /* S^-1 */
+    kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
+    kf->temp_matrix.numRows = kf->Pminus.numRows;
+    kf->temp_matrix.numCols = kf->HT.numCols;
+    /* P'(k)·H^T */
+    kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
+    /* K = P'(k)·H^T·S^-1 */
+    kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k))
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_UpdateState(KF_t *kf) {
+  if (kf == NULL) return -1;
+
+  if (!kf->skip_eq4) {
+    kf->temp_vector.numRows = kf->H.numRows;
+    kf->temp_vector.numCols = 1;
+    /* H·xhat'(k) */
+    kf->mat_status = KF_MatMult(&kf->H, &kf->xhatminus, &kf->temp_vector);
+    kf->temp_vector1.numRows = kf->z.numRows;
+    kf->temp_vector1.numCols = 1;
+    /* 新息: z - H·xhat'(k) */
+    kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
+    kf->temp_vector.numRows = kf->K.numRows;
+    kf->temp_vector.numCols = 1;
+    /* K·新息 */
+    kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
+    /* xhat = xhat' + K·新息 */
+    kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k)
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_UpdateCovariance(KF_t *kf) {
+  if (kf == NULL) return -1;
+
+  if (!kf->skip_eq5) {
+    kf->temp_matrix.numRows = kf->K.numRows;
+    kf->temp_matrix.numCols = kf->H.numCols;
+    kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
+    kf->temp_matrix1.numCols = kf->Pminus.numCols;
+    /* K·H */
+    kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix);
+    /* K·H·P'(k) */
+    kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1);
+    /* P = P' - K·H·P' */
+    kf->mat_status = KF_MatSub(&kf->Pminus, &kf->temp_matrix1, &kf->P);
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 执行完整的卡尔曼滤波周期（五大方程）
+ *
+ * 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。
+ * 每个步骤都可以通过 User_Func 回调函数替换。
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return float* 滤波后的状态估计值指针
+ */
+float *KF_Update(KF_t *kf) {
+  if (kf == NULL) return NULL;
+
+  /* 步骤0: 量测获取和矩阵调整 */
+  KF_Measure(kf);
+  if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf);
+
+  /* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */
+  KF_PredictState(kf);
+  if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf);
+
+  /* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */
+  KF_PredictCovariance(kf);
+  if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf);
+
+  /* 量测更新（仅在存在有效量测时执行） */
+  if (kf->measurement_valid_num != 0 || kf->use_auto_adjustment == 0) {
+    /* 步骤3: 卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) */
+    KF_CalcGain(kf);
+    if (kf->User_Func3_f != NULL) kf->User_Func3_f(kf);
+
+    /* 步骤4: 状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) */
+    KF_UpdateState(kf);
+    if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf);
+
+    /* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */
+    KF_UpdateCovariance(kf);
+  } else {
+    /* 无有效量测 - 仅预测 */
+    memcpy(kf->xhat_data, kf->xhatminus_data, sizeof(float) * kf->xhat_size);
+    memcpy(kf->P_data, kf->Pminus_data,
+           sizeof(float) * kf->xhat_size * kf->xhat_size);
+  }
+
+  /* 后处理钩子 */
+  if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf);
+
+  /* 防过度收敛：强制最小方差 */
+  for (uint8_t i = 0; i < kf->xhat_size; i++) {
+    if (kf->P_data[i * kf->xhat_size + i] < kf->state_min_variance[i])
+      kf->P_data[i * kf->xhat_size + i] = kf->state_min_variance[i];
+  }
+
+  /* 复制结果到输出 */
+  memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size);
+
+  /* 附加后处理钩子 */
+  if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf);
+
+  return kf->filtered_value;
+}
+
+/**
+ * @brief 重置卡尔曼滤波器状态
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+void KF_Reset(KF_t *kf) {
+  if (kf == NULL) return;
+
+  memset(kf->xhat_data, 0, sizeof(float) * kf->xhat_size);
+  memset(kf->xhatminus_data, 0, sizeof(float) * kf->xhat_size);
+  memset(kf->filtered_value, 0, sizeof(float) * kf->xhat_size);
+  kf->measurement_valid_num = 0;
+}
+
+/**
+ * @brief 根据量测有效性动态调整 H, R, K 矩阵
+ *
+ * 该函数根据当前周期中哪些量测有效（非零）来重建量测相关矩阵。
+ * 支持具有不同采样率的异步传感器。
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+static void KF_AdjustHKR(KF_t *kf) {
+  kf->measurement_valid_num = 0;
+
+  /* 复制并重置量测向量 */
+  memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size);
+  memset(kf->measured_vector, 0, sizeof(float) * kf->z_size);
+
+  /* 清空 H 和 R 矩阵 */
+  memset(kf->R_data, 0, sizeof(float) * kf->z_size * kf->z_size);
+  memset(kf->H_data, 0, sizeof(float) * kf->xhat_size * kf->z_size);
+
+  /* 识别有效量测并重建 z, H */
+  for (uint8_t i = 0; i < kf->z_size; i++) {
+    if (kf->z_data[i] != 0) { /* 非零表示有效量测 */
+      /* 将有效量测压缩到 z */
+      kf->z_data[kf->measurement_valid_num] = kf->z_data[i];
+      kf->temp[kf->measurement_valid_num] = i;
+
+      /* 重建 H 矩阵行 */
+      kf->H_data[kf->xhat_size * kf->measurement_valid_num +
+                 kf->measurement_map[i] - 1] = kf->measurement_degree[i];
+      kf->measurement_valid_num++;
+    }
+  }
+
+  /* 用有效量测方差重建 R 矩阵 */
+  for (uint8_t i = 0; i < kf->measurement_valid_num; i++) {
+    kf->R_data[i * kf->measurement_valid_num + i] =
+        kf->mat_r_diagonal_elements[kf->temp[i]];
+  }
+
+  /* 调整矩阵维度以匹配有效量测数量 */
+  kf->H.numRows = kf->measurement_valid_num;
+  kf->H.numCols = kf->xhat_size;
+  kf->HT.numRows = kf->xhat_size;
+  kf->HT.numCols = kf->measurement_valid_num;
+  kf->R.numRows = kf->measurement_valid_num;
+  kf->R.numCols = kf->measurement_valid_num;
+  kf->K.numRows = kf->xhat_size;
+  kf->K.numCols = kf->measurement_valid_num;
+  kf->z.numRows = kf->measurement_valid_num;
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

User/component/kalman_filter.h

@@ -0,0 +1,199 @@
+/*
+  卡尔曼滤波器
+  支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "arm_math.h"
+
+#include <math.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 内存分配配置 */
+#ifndef user_malloc
+#ifdef _CMSIS_OS_H
+#define user_malloc pvPortMalloc /* FreeRTOS堆分配 */
+#else
+#define user_malloc malloc /* 标准C库分配 */
+#endif
+#endif
+
+/* ARM CMSIS DSP 矩阵运算别名 */
+#define KF_Mat              arm_matrix_instance_f32
+#define KF_MatInit          arm_mat_init_f32
+#define KF_MatAdd           arm_mat_add_f32
+#define KF_MatSub           arm_mat_sub_f32
+#define KF_MatMult          arm_mat_mult_f32
+#define KF_MatTrans         arm_mat_trans_f32
+#define KF_MatInv           arm_mat_inverse_f32
+
+/* 卡尔曼滤波器主结构体 */
+typedef struct KF_s {
+  /* 输出和输入向量 */
+  float *filtered_value;   /* 滤波后的状态估计输出 */
+  float *measured_vector;  /* 量测输入向量 */
+  float *control_vector;   /* 控制输入向量 */
+
+  /* 维度 */
+  uint8_t xhat_size; /* 状态向量维度 */
+  uint8_t u_size;    /* 控制向量维度 */
+  uint8_t z_size;    /* 量测向量维度 */
+
+  /* 自动调整参数 */
+  uint8_t use_auto_adjustment;    /* 启用动态 H/R/K 调整 */
+  uint8_t measurement_valid_num;  /* 有效量测数量 */
+
+  /* 量测配置 */
+  uint8_t *measurement_map;        /* 量测到状态的映射 */
+  float *measurement_degree;       /* 每个量测的H矩阵元素值 */
+  float *mat_r_diagonal_elements;  /* 量测噪声方差（R对角线） */
+  float *state_min_variance;       /* 最小状态方差（防过度收敛） */
+  uint8_t *temp;                   /* 临时缓冲区 */
+
+  /* 方程跳过标志（用于自定义用户函数） */
+  uint8_t skip_eq1, skip_eq2, skip_eq3, skip_eq4, skip_eq5;
+
+  /* 卡尔曼滤波器矩阵 */
+  KF_Mat xhat;      /* 状态估计 x(k|k) */
+  KF_Mat xhatminus; /* 先验状态估计 x(k|k-1) */
+  KF_Mat u;         /* 控制向量 */
+  KF_Mat z;         /* 量测向量 */
+  KF_Mat P;         /* 后验误差协方差 P(k|k) */
+  KF_Mat Pminus;    /* 先验误差协方差 P(k|k-1) */
+  KF_Mat F, FT;     /* 状态转移矩阵及其转置 */
+  KF_Mat B;         /* 控制矩阵 */
+  KF_Mat H, HT;     /* 量测矩阵及其转置 */
+  KF_Mat Q;         /* 过程噪声协方差 */
+  KF_Mat R;         /* 量测噪声协方差 */
+  KF_Mat K;         /* 卡尔曼增益 */
+  KF_Mat S;         /* 临时矩阵 S */
+  KF_Mat temp_matrix, temp_matrix1;   /* 临时矩阵 */
+  KF_Mat temp_vector, temp_vector1;   /* 临时向量 */
+
+  int8_t mat_status; /* 矩阵运算状态 */
+
+  /* 用户自定义函数指针（用于EKF/UKF/ESKF扩展） */
+  void (*User_Func0_f)(struct KF_s *kf); /* 自定义量测处理 */
+  void (*User_Func1_f)(struct KF_s *kf); /* 自定义状态预测 */
+  void (*User_Func2_f)(struct KF_s *kf); /* 自定义协方差预测 */
+  void (*User_Func3_f)(struct KF_s *kf); /* 自定义卡尔曼增益计算 */
+  void (*User_Func4_f)(struct KF_s *kf); /* 自定义状态更新 */
+  void (*User_Func5_f)(struct KF_s *kf); /* 自定义后处理 */
+  void (*User_Func6_f)(struct KF_s *kf); /* 附加自定义函数 */
+
+  /* 矩阵数据存储指针 */
+  float *xhat_data, *xhatminus_data;
+  float *u_data;
+  float *z_data;
+  float *P_data, *Pminus_data;
+  float *F_data, *FT_data;
+  float *B_data;
+  float *H_data, *HT_data;
+  float *Q_data;
+  float *R_data;
+  float *K_data;
+  float *S_data;
+  float *temp_matrix_data, *temp_matrix_data1;
+  float *temp_vector_data, *temp_vector_data1;
+} KF_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief 初始化卡尔曼滤波器并分配矩阵内存
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @param xhat_size 状态向量维度
+ * @param u_size 控制向量维度（无控制输入时设为0）
+ * @param z_size 量测向量维度
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size);
+
+/**
+ * @brief 获取量测并在启用自动调整时调整矩阵
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_Measure(KF_t *kf);
+
+/**
+ * @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_PredictState(KF_t *kf);
+
+/**
+ * @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_PredictCovariance(KF_t *kf);
+
+/**
+ * @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_CalcGain(KF_t *kf);
+
+/**
+ * @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k))
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_UpdateState(KF_t *kf);
+
+/**
+ * @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k)
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return int8_t 0对应没有错误
+ */
+int8_t KF_UpdateCovariance(KF_t *kf);
+
+/**
+ * @brief 执行完整的卡尔曼滤波周期（五大方程）
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ * @return float* 滤波后的状态估计值指针
+ */
+float *KF_Update(KF_t *kf);
+
+/**
+ * @brief 重置卡尔曼滤波器状态
+ *
+ * @param kf 卡尔曼滤波器结构体指针
+ */
+void KF_Reset(KF_t *kf);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

User/component/vmc.c

@@ -61,6 +61,9 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
     // 重置状态
     VMC_Reset(vmc);
     
+    // 初始化dd_theta低通滤波器 (截止频率50Hz)
+    LowPassFilter2p_Init(&vmc->dd_theta_filter, sample_freq, 50.0f);
+    
     vmc->initialized = true;
     
     return 0;
@@ -124,8 +127,9 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     leg->d_theta = body_pitch_rate + leg->d_phi0;
     leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
     
-    // 计算角加速度
+    // 计算角加速度并滤波
-    leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
+    float dd_theta_raw = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
+    leg->dd_theta = LowPassFilter2p_Apply(&vmc->dd_theta_filter, dd_theta_raw);
     
       VMC_GroundContactDetection(vmc); // 更新地面接触状态
     return 0;

User/component/vmc.h

@@ -8,6 +8,7 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 #include <math.h>
+#include "filter.h"
 /* Exported types ----------------------------------------------------------- */
 
 /**
@@ -83,6 +84,7 @@ typedef struct {
     VMC_Leg_t leg;         // 腿部状态
     float dt;              // 控制周期
     bool initialized;      // 初始化标志
+    LowPassFilter2p_t dd_theta_filter;  // dd_theta低通滤波器
 } VMC_t;
 
 /* Exported constants ------------------------------------------------------- */

User/device/bmi088.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <sys/_intsup.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -30,6 +31,7 @@ typedef struct {
   } gyro_offset; /* 陀螺仪偏置 */
 } BMI088_Cali_t; /* BMI088校准数据 */
 
+
 typedef struct {
   DEVICE_Header_t header;
   AHRS_Accl_t accl;

User/device/device.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <sys/_intsup.h>
 #ifdef __cplusplus
 extern "C" {
 #endif

User/device/device_config.yaml

@@ -29,3 +29,7 @@ motor_lz:
 motor_rm:
   bsp_config: {}
   enabled: true
+vofa:
+  bsp_config:
+    BSP_UART_VOFA: BSP_UART_VOFA
+  enabled: true

User/device/mrobot.c

@@ -0,0 +1,873 @@
+/**
+ * @file mrobot.c
+ * @brief MRobot CLI 实现
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/mrobot.h"
+#include "component/freertos_cli.h"
+#include "bsp/uart.h"
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <FreeRTOS.h>
+#include <task.h>
+#include <semphr.h>
+#include <cmsis_os2.h>
+
+/* Private constants -------------------------------------------------------- */
+static const char *const CLI_WELCOME_MESSAGE =
+    "\r\n"
+    "  __  __ _____       _           _   \r\n"
+    " |  \\/  |  __ \\     | |         | |  \r\n"
+    " | \\  / | |__) |___ | |__   ___ | |_ \r\n"
+    " | |\\/| |  _  // _ \\| '_ \\ / _ \\| __|\r\n"
+    " | |  | | | \\ \\ (_) | |_) | (_) | |_ \r\n"
+    " |_|  |_|_|  \\_\\___/|_.__/ \\___/ \\__|\r\n"
+    " ------------------------------------\r\n"
+    " Welcome to use MRobot CLI. Type 'help' to view a list of registered commands.\r\n"
+    "\r\n";
+
+/* ANSI 转义序列 */
+#define ANSI_CLEAR_SCREEN   "\033[2J\033[H"
+#define ANSI_CURSOR_HOME    "\033[H"
+#define ANSI_BACKSPACE      "\b \b"
+
+/* Private types ------------------------------------------------------------ */
+/* CLI 上下文结构体 - 封装所有状态 */
+typedef struct {
+    /* 设备管理 */
+    MRobot_Device_t devices[MROBOT_MAX_DEVICES];
+    uint8_t device_count;
+    
+    /* 自定义命令 */
+    CLI_Command_Definition_t *custom_cmds[MROBOT_MAX_CUSTOM_COMMANDS];
+    uint8_t custom_cmd_count;
+    
+    /* CLI 状态 */
+    MRobot_State_t state;
+    char current_path[MROBOT_PATH_MAX_LEN];
+    
+    /* 命令缓冲区 */
+    uint8_t cmd_buffer[MROBOT_CMD_BUFFER_SIZE];
+    volatile uint8_t cmd_index;
+    volatile bool cmd_ready;
+    
+    /* UART 相关 */
+    uint8_t uart_rx_char;
+    volatile bool tx_complete;
+    volatile bool htop_exit;
+    
+    /* 输出缓冲区 */
+    char output_buffer[MROBOT_OUTPUT_BUFFER_SIZE];
+    
+    /* 初始化标志 */
+    bool initialized;
+    
+    /* 互斥锁 */
+    SemaphoreHandle_t mutex;
+} MRobot_Context_t;
+
+/* Private variables -------------------------------------------------------- */
+static MRobot_Context_t ctx = {
+    .device_count = 0,
+    .custom_cmd_count = 0,
+    .state = MROBOT_STATE_IDLE,
+    .current_path = "/",
+    .cmd_index = 0,
+    .cmd_ready = false,
+    .tx_complete = true,
+    .htop_exit = false,
+    .initialized = false,
+    .mutex = NULL
+};
+
+/* Private function prototypes ---------------------------------------------- */
+/* 命令处理函数 */
+static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
+static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
+static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
+static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
+static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
+
+/* 内部辅助函数 */
+static void uart_tx_callback(void);
+static void uart_rx_callback(void);
+static void send_string(const char *str);
+static void send_prompt(void);
+static int format_float_va(char *buf, size_t size, const char *fmt, va_list args);
+
+/* CLI 命令定义表 */
+static const CLI_Command_Definition_t builtin_commands[] = {
+    { "help", " --help: 显示所有可用命令\r\n", cmd_help, 0 },
+    { "htop", " --htop: 动态显示 FreeRTOS 任务状态 (按 'q' 退出)\r\n", cmd_htop, 0 },
+    { "cd",   " --cd <path>: 切换目录\r\n", cmd_cd, 1 },
+    { "ls",   " --ls: 列出当前目录内容\r\n", cmd_ls, 0 },
+    { "show", " --show [device] [count]: 显示设备信息\r\n", cmd_show, -1 },
+};
+#define BUILTIN_CMD_COUNT (sizeof(builtin_commands) / sizeof(builtin_commands[0]))
+
+/* ========================================================================== */
+/*                              辅助函数实现                                    */
+/* ========================================================================== */
+
+/**
+ * @brief 发送字符串到 UART（阻塞等待完成）
+ */
+static void send_string(const char *str) {
+    if (str == NULL || *str == '\0') return;
+    
+    ctx.tx_complete = false;
+    BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)str, strlen(str), true);
+    while (!ctx.tx_complete) { osDelay(1); }
+}
+
+/**
+ * @brief 发送命令提示符
+ */
+static void send_prompt(void) {
+    char prompt[MROBOT_PATH_MAX_LEN + 32];
+    snprintf(prompt, sizeof(prompt), MROBOT_USER_NAME "@" MROBOT_HOST_NAME ":%s$ ", ctx.current_path);
+    send_string(prompt);
+}
+
+/**
+ * @brief UART 发送完成回调
+ */
+static void uart_tx_callback(void) {
+    ctx.tx_complete = true;
+}
+
+/**
+ * @brief UART 接收回调
+ */
+static void uart_rx_callback(void) {
+    uint8_t ch = ctx.uart_rx_char;
+    
+    /* htop 模式下检查退出键 */
+    if (ctx.state == MROBOT_STATE_HTOP) {
+        if (ch == 'q' || ch == 'Q' || ch == 27) {
+            ctx.htop_exit = true;
+        }
+        BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
+        return;
+    }
+    
+    /* 正常命令输入处理 */
+    if (ch == '\r' || ch == '\n') {
+        if (ctx.cmd_index > 0) {
+            ctx.cmd_buffer[ctx.cmd_index] = '\0';
+            ctx.cmd_ready = true;
+            BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)"\r\n", 2, false);
+        }
+    } else if (ch == 127 || ch == 8) {  /* 退格键 */
+        if (ctx.cmd_index > 0) {
+            ctx.cmd_index--;
+            BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)ANSI_BACKSPACE, 3, false);
+        }
+    } else if (ch >= 32 && ch < 127 && ctx.cmd_index < sizeof(ctx.cmd_buffer) - 1) {
+        ctx.cmd_buffer[ctx.cmd_index++] = ch;
+        BSP_UART_Transmit(MROBOT_UART_PORT, &ch, 1, false);
+    }
+    
+    BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
+}
+
+/* ========================================================================== */
+/*                             CLI 命令实现                                     */
+/* ========================================================================== */
+
+/**
+ * @brief help 命令 - 显示帮助信息
+ */
+static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
+    (void)pcCommandString;
+    
+    int offset = snprintf(pcWriteBuffer, xWriteBufferLen,
+        "MRobot CLI v2.0\r\n"
+        "================\r\n"
+        "Built-in Commands:\r\n");
+    
+    for (size_t i = 0; i < BUILTIN_CMD_COUNT && offset < (int)xWriteBufferLen - 50; i++) {
+        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+            "  %s", builtin_commands[i].pcHelpString);
+    }
+    
+    if (ctx.custom_cmd_count > 0) {
+        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+            "\r\nCustom Commands:\r\n");
+        for (uint8_t i = 0; i < ctx.custom_cmd_count && offset < (int)xWriteBufferLen - 50; i++) {
+            if (ctx.custom_cmds[i] != NULL) {
+                offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                    "  %s", ctx.custom_cmds[i]->pcHelpString);
+            }
+        }
+    }
+    
+    return pdFALSE;
+}
+
+/**
+ * @brief htop 命令 - 设置 htop 模式标志
+ */
+static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
+    (void)pcCommandString;
+    (void)pcWriteBuffer;
+    (void)xWriteBufferLen;
+    /* htop 模式在 MRobot_Run 中处理 */
+    return pdFALSE;
+}
+
+/**
+ * @brief cd 命令 - 切换目录
+ */
+static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
+    const char *param;
+    BaseType_t param_len;
+    
+    param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
+    
+    if (param == NULL) {
+        /* 无参数时切换到根目录 */
+        strcpy(ctx.current_path, "/");
+        snprintf(pcWriteBuffer, xWriteBufferLen, "Changed to: %s\r\n", ctx.current_path);
+        return pdFALSE;
+    }
+    
+    /* 安全复制路径参数 */
+    char path[MROBOT_PATH_MAX_LEN];
+    size_t copy_len = (size_t)param_len < sizeof(path) - 1 ? (size_t)param_len : sizeof(path) - 1;
+    strncpy(path, param, copy_len);
+    path[copy_len] = '\0';
+    
+    /* 路径解析 */
+    if (strcmp(path, "/") == 0 || strcmp(path, "..") == 0 || strcmp(path, "~") == 0) {
+        strcpy(ctx.current_path, "/");
+    } else if (strcmp(path, "dev") == 0 || strcmp(path, "/dev") == 0) {
+        strcpy(ctx.current_path, "/dev");
+    } else if (strcmp(path, "modules") == 0 || strcmp(path, "/modules") == 0) {
+        strcpy(ctx.current_path, "/modules");
+    } else {
+        snprintf(pcWriteBuffer, xWriteBufferLen, "Error: Directory '%s' not found\r\n", path);
+        return pdFALSE;
+    }
+    
+    snprintf(pcWriteBuffer, xWriteBufferLen, "Changed to: %s\r\n", ctx.current_path);
+    return pdFALSE;
+}
+
+/**
+ * @brief ls 命令 - 列出目录内容
+ */
+static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
+    (void)pcCommandString;
+    int offset = 0;
+    
+    if (strcmp(ctx.current_path, "/") == 0) {
+        snprintf(pcWriteBuffer, xWriteBufferLen, 
+            "dev/\r\n"
+            "modules/\r\n");
+    } else if (strcmp(ctx.current_path, "/dev") == 0) {
+        offset = snprintf(pcWriteBuffer, xWriteBufferLen, 
+            "Device List (%d devices)\r\n\r\n", 
+            ctx.device_count);
+        
+        if (ctx.device_count == 0) {
+            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, 
+                              "  (No devices)\r\n");
+        } else {
+            /* 直接列出所有设备 */
+            for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 50; i++) {
+                offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                    "  - %s\r\n", ctx.devices[i].name);
+            }
+        }
+    } else if (strcmp(ctx.current_path, "/modules") == 0) {
+        snprintf(pcWriteBuffer, xWriteBufferLen, 
+            "Module functions not yet implemented\r\n");
+    }
+    
+    return pdFALSE;
+}
+
+/**
+ * @brief show 命令 - 显示设备信息
+ */
+static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
+    const char *param;
+    const char *count_param;
+    BaseType_t param_len, count_param_len;
+    
+    /* 使用局部静态变量跟踪多次打印状态 */
+    static uint32_t print_count = 0;
+    static uint32_t current_iter = 0;
+    static char target_device[MROBOT_DEVICE_NAME_LEN] = {0};
+    
+    /* 首次调用时解析参数 */
+    if (current_iter == 0) {
+        /* 检查是否在 /dev 目录 */
+        if (strcmp(ctx.current_path, "/dev") != 0) {
+            snprintf(pcWriteBuffer, xWriteBufferLen, 
+                "Error: 'show' command only works in /dev directory\r\n"
+                "Hint: Use 'cd /dev' to switch to device directory\r\n");
+            return pdFALSE;
+        }
+        
+        param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
+        count_param = FreeRTOS_CLIGetParameter(pcCommandString, 2, &count_param_len);
+        
+        /* 解析打印次数 */
+        print_count = 1;
+        if (count_param != NULL) {
+            char count_str[16];
+            size_t copy_len = (size_t)count_param_len < sizeof(count_str) - 1 ? 
+                              (size_t)count_param_len : sizeof(count_str) - 1;
+            strncpy(count_str, count_param, copy_len);
+            count_str[copy_len] = '\0';
+            int parsed = atoi(count_str);
+            if (parsed > 0 && parsed <= 1000) {
+                print_count = (uint32_t)parsed;
+            }
+        }
+        
+        /* 保存目标设备名称 */
+        if (param != NULL) {
+            size_t copy_len = (size_t)param_len < sizeof(target_device) - 1 ? 
+                              (size_t)param_len : sizeof(target_device) - 1;
+            strncpy(target_device, param, copy_len);
+            target_device[copy_len] = '\0';
+        } else {
+            target_device[0] = '\0';
+        }
+    }
+    
+    int offset = 0;
+    
+    /* 连续打印模式：清屏 */
+    if (print_count > 1) {
+        offset = snprintf(pcWriteBuffer, xWriteBufferLen, "%s[%lu/%lu]\r\n",
+                          ANSI_CLEAR_SCREEN,
+                          (unsigned long)(current_iter + 1), 
+                          (unsigned long)print_count);
+    }
+    
+    if (target_device[0] == '\0') {
+        /* 显示所有设备 */
+        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                          "=== All Devices ===\r\n\r\n");
+        
+        for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 100; i++) {
+            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                              "--- %s ---\r\n", ctx.devices[i].name);
+            
+            if (ctx.devices[i].print_cb != NULL) {
+                int written = ctx.devices[i].print_cb(ctx.devices[i].data,
+                                                       pcWriteBuffer + offset,
+                                                       xWriteBufferLen - offset);
+                offset += (written > 0) ? written : 0;
+            } else {
+                offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                                  "  (No print function)\r\n");
+            }
+        }
+        
+        if (ctx.device_count == 0) {
+            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                              "  (No devices registered)\r\n");
+        }
+    } else {
+        /* 显示指定设备 */
+        const MRobot_Device_t *dev = MRobot_FindDevice(target_device);
+        
+        if (dev == NULL) {
+            snprintf(pcWriteBuffer, xWriteBufferLen, 
+                    "Error: Device '%s' not found\r\n", 
+                    target_device);
+            current_iter = 0;
+            return pdFALSE;
+        }
+        
+        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                          "=== %s ===\r\n", dev->name);
+        
+        if (dev->print_cb != NULL) {
+            dev->print_cb(dev->data, pcWriteBuffer + offset, xWriteBufferLen - offset);
+        } else {
+            snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
+                    "  (No print function)\r\n");
+        }
+    }
+    
+    /* 判断是否继续打印 */
+    current_iter++;
+    if (current_iter < print_count) {
+        osDelay(MROBOT_HTOP_REFRESH_MS);
+        return pdTRUE;
+    } else {
+        current_iter = 0;
+        return pdFALSE;
+    }
+}
+
+/* ============================================================================
+ * htop 模式处理
+ * ========================================================================== */
+static void handle_htop_mode(void) {
+    send_string(ANSI_CLEAR_SCREEN);
+    send_string("=== MRobot Task Monitor (Press 'q' to exit) ===\r\n\r\n");
+    
+    /* 获取任务列表 */
+    char task_buffer[1024];
+    char display_line[128];
+    
+    vTaskList(task_buffer);
+    
+    /* 表头 */
+    send_string("Task Name         State    Prio  Stack  Num\r\n");
+    send_string("------------------------------------------------\r\n");
+    
+    /* 解析并格式化任务列表 */
+    char *line = strtok(task_buffer, "\r\n");
+    while (line != NULL) {
+        char name[17] = {0};
+        char state_char = '?';
+        int prio = 0, stack = 0, num = 0;
+        
+        if (sscanf(line, "%16s %c %d %d %d", name, &state_char, &prio, &stack, &num) == 5) {
+            const char *state_str;
+            switch (state_char) {
+                case 'R': state_str = "Running"; break;
+                case 'B': state_str = "Blocked"; break;
+                case 'S': state_str = "Suspend"; break;
+                case 'D': state_str = "Deleted"; break;
+                case 'X': state_str = "Ready  "; break;
+                default:  state_str = "Unknown"; break;
+            }
+            
+            snprintf(display_line, sizeof(display_line),
+                    "%-16s %-8s %-4d %-8d %-4d\r\n",
+                    name, state_str, prio, stack, num);
+            send_string(display_line);
+        }
+        line = strtok(NULL, "\r\n");
+    }
+    
+    /* 显示系统信息 */
+    snprintf(display_line, sizeof(display_line),
+            "------------------------------------------------\r\n"
+            "System Tick: %lu | Free Heap: %lu bytes\r\n",
+            (unsigned long)xTaskGetTickCount(),
+            (unsigned long)xPortGetFreeHeapSize());
+    send_string(display_line);
+    
+    /* 检查退出 */
+    if (ctx.htop_exit) {
+        ctx.state = MROBOT_STATE_IDLE;
+        ctx.htop_exit = false;
+        send_string(ANSI_CLEAR_SCREEN);
+        send_prompt();
+    }
+    
+    osDelay(MROBOT_HTOP_REFRESH_MS);
+}
+
+/* ========================================================================== */
+/*                              公共 API 实现                                   */
+/* ========================================================================== */
+
+void MRobot_Init(void) {
+    if (ctx.initialized) return;
+    
+    /* 创建互斥锁 */
+    ctx.mutex = xSemaphoreCreateMutex();
+    
+    /* 初始化状态 */
+    memset(ctx.devices, 0, sizeof(ctx.devices));
+    ctx.device_count = 0;
+    ctx.custom_cmd_count = 0;
+    ctx.state = MROBOT_STATE_IDLE;
+    strcpy(ctx.current_path, "/");
+    ctx.cmd_index = 0;
+    ctx.cmd_ready = false;
+    ctx.tx_complete = true;
+    ctx.htop_exit = false;
+    
+    /* 注册内置命令 */
+    for (size_t i = 0; i < BUILTIN_CMD_COUNT; i++) {
+        FreeRTOS_CLIRegisterCommand(&builtin_commands[i]);
+    }
+    
+    /* 注册 UART 回调 */
+    BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_RX_CPLT_CB, uart_rx_callback);
+    BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_TX_CPLT_CB, uart_tx_callback);
+    
+    /* 启动 UART 接收 */
+    BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
+    
+    /* 等待用户按下回车 */
+    while (ctx.uart_rx_char != '\r' && ctx.uart_rx_char != '\n') {
+        osDelay(10);
+    }
+    
+    /* 发送欢迎消息和提示符 */
+    send_string(CLI_WELCOME_MESSAGE);
+    send_prompt();
+    
+    ctx.initialized = true;
+}
+
+void MRobot_DeInit(void) {
+    if (!ctx.initialized) return;
+    
+    /* 释放自定义命令内存 */
+    for (uint8_t i = 0; i < ctx.custom_cmd_count; i++) {
+        if (ctx.custom_cmds[i] != NULL) {
+            vPortFree(ctx.custom_cmds[i]);
+            ctx.custom_cmds[i] = NULL;
+        }
+    }
+    
+    /* 删除互斥锁 */
+    if (ctx.mutex != NULL) {
+        vSemaphoreDelete(ctx.mutex);
+        ctx.mutex = NULL;
+    }
+    
+    ctx.initialized = false;
+}
+
+MRobot_State_t MRobot_GetState(void) {
+    return ctx.state;
+}
+
+MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb) {
+    if (name == NULL || data == NULL) {
+        return MROBOT_ERR_NULL_PTR;
+    }
+    
+    if (ctx.device_count >= MROBOT_MAX_DEVICES) {
+        return MROBOT_ERR_FULL;
+    }
+    
+    /* 检查重名 */
+    for (uint8_t i = 0; i < ctx.device_count; i++) {
+        if (strcmp(ctx.devices[i].name, name) == 0) {
+            return MROBOT_ERR_INVALID_ARG;  /* 设备名已存在 */
+        }
+    }
+    
+    /* 线程安全写入 */
+    if (ctx.mutex != NULL) {
+        xSemaphoreTake(ctx.mutex, portMAX_DELAY);
+    }
+    
+    strncpy(ctx.devices[ctx.device_count].name, name, MROBOT_DEVICE_NAME_LEN - 1);
+    ctx.devices[ctx.device_count].name[MROBOT_DEVICE_NAME_LEN - 1] = '\0';
+    ctx.devices[ctx.device_count].data = data;
+    ctx.devices[ctx.device_count].print_cb = print_cb;
+    ctx.device_count++;
+    
+    if (ctx.mutex != NULL) {
+        xSemaphoreGive(ctx.mutex);
+    }
+    
+    return MROBOT_OK;
+}
+
+MRobot_Error_t MRobot_UnregisterDevice(const char *name) {
+    if (name == NULL) {
+        return MROBOT_ERR_NULL_PTR;
+    }
+    
+    if (ctx.mutex != NULL) {
+        xSemaphoreTake(ctx.mutex, portMAX_DELAY);
+    }
+    
+    for (uint8_t i = 0; i < ctx.device_count; i++) {
+        if (strcmp(ctx.devices[i].name, name) == 0) {
+            /* 移动后续设备 */
+            for (uint8_t j = i; j < ctx.device_count - 1; j++) {
+                ctx.devices[j] = ctx.devices[j + 1];
+            }
+            ctx.device_count--;
+            
+            if (ctx.mutex != NULL) {
+                xSemaphoreGive(ctx.mutex);
+            }
+            return MROBOT_OK;
+        }
+    }
+    
+    if (ctx.mutex != NULL) {
+        xSemaphoreGive(ctx.mutex);
+    }
+    return MROBOT_ERR_NOT_FOUND;
+}
+
+MRobot_Error_t MRobot_RegisterCommand(const char *command, const char *help_text,
+                                       MRobot_CommandCallback_t callback, int8_t param_count) {
+    if (command == NULL || help_text == NULL || callback == NULL) {
+        return MROBOT_ERR_NULL_PTR;
+    }
+    
+    if (ctx.custom_cmd_count >= MROBOT_MAX_CUSTOM_COMMANDS) {
+        return MROBOT_ERR_FULL;
+    }
+    
+    /* 动态分配命令结构体 */
+    CLI_Command_Definition_t *cmd_def = pvPortMalloc(sizeof(CLI_Command_Definition_t));
+    if (cmd_def == NULL) {
+        return MROBOT_ERR_ALLOC;
+    }
+    
+    /* 初始化命令定义 */
+    *(const char **)&cmd_def->pcCommand = command;
+    *(const char **)&cmd_def->pcHelpString = help_text;
+    *(pdCOMMAND_LINE_CALLBACK *)&cmd_def->pxCommandInterpreter = (pdCOMMAND_LINE_CALLBACK)callback;
+    cmd_def->cExpectedNumberOfParameters = param_count;
+    
+    /* 注册到 FreeRTOS CLI */
+    FreeRTOS_CLIRegisterCommand(cmd_def);
+    
+    ctx.custom_cmds[ctx.custom_cmd_count] = cmd_def;
+    ctx.custom_cmd_count++;
+    
+    return MROBOT_OK;
+}
+
+uint8_t MRobot_GetDeviceCount(void) {
+    return ctx.device_count;
+}
+
+const MRobot_Device_t *MRobot_FindDevice(const char *name) {
+    if (name == NULL) return NULL;
+    
+    for (uint8_t i = 0; i < ctx.device_count; i++) {
+        if (strcmp(ctx.devices[i].name, name) == 0) {
+            return &ctx.devices[i];
+        }
+    }
+    return NULL;
+}
+
+int MRobot_Printf(const char *fmt, ...) {
+    if (fmt == NULL || !ctx.initialized) return -1;
+    
+    char buffer[MROBOT_OUTPUT_BUFFER_SIZE];
+    va_list args;
+    va_start(args, fmt);
+    int len = format_float_va(buffer, sizeof(buffer), fmt, args);
+    va_end(args);
+    
+    if (len > 0) {
+        send_string(buffer);
+    }
+    return len;
+}
+
+/**
+ * @brief 内部函数：格式化浮点数到缓冲区（va_list 版本）
+ */
+static int format_float_va(char *buf, size_t size, const char *fmt, va_list args) {
+    if (buf == NULL || size == 0 || fmt == NULL) return 0;
+    
+    char *buf_ptr = buf;
+    size_t buf_remain = size - 1;
+    
+    const char *p = fmt;
+    while (*p && buf_remain > 0) {
+        if (*p != '%') {
+            *buf_ptr++ = *p++;
+            buf_remain--;
+            continue;
+        }
+        
+        p++;  /* 跳过 '%' */
+        
+        /* 处理 %% */
+        if (*p == '%') {
+            *buf_ptr++ = '%';
+            buf_remain--;
+            p++;
+            continue;
+        }
+        
+        /* 解析精度 %.Nf */
+        int precision = 2;  /* 默认精度 */
+        if (*p == '.') {
+            p++;
+            precision = 0;
+            while (*p >= '0' && *p <= '9') {
+                precision = precision * 10 + (*p - '0');
+                p++;
+            }
+            if (precision > 6) precision = 6;
+        }
+        
+        int written = 0;
+        switch (*p) {
+            case 'f': {  /* 浮点数 */
+                double val = va_arg(args, double);
+                written = MRobot_FormatFloat(buf_ptr, buf_remain, (float)val, precision);
+                break;
+            }
+            case 'd':
+            case 'i': {
+                int val = va_arg(args, int);
+                written = snprintf(buf_ptr, buf_remain, "%d", val);
+                break;
+            }
+            case 'u': {
+                unsigned int val = va_arg(args, unsigned int);
+                written = snprintf(buf_ptr, buf_remain, "%u", val);
+                break;
+            }
+            case 'x': {
+                unsigned int val = va_arg(args, unsigned int);
+                written = snprintf(buf_ptr, buf_remain, "%x", val);
+                break;
+            }
+            case 'X': {
+                unsigned int val = va_arg(args, unsigned int);
+                written = snprintf(buf_ptr, buf_remain, "%X", val);
+                break;
+            }
+            case 's': {
+                const char *str = va_arg(args, const char *);
+                if (str == NULL) str = "(null)";
+                written = snprintf(buf_ptr, buf_remain, "%s", str);
+                break;
+            }
+            case 'c': {
+                int ch = va_arg(args, int);
+                *buf_ptr = (char)ch;
+                written = 1;
+                break;
+            }
+            case 'l': {
+                p++;
+                if (*p == 'd' || *p == 'i') {
+                    long val = va_arg(args, long);
+                    written = snprintf(buf_ptr, buf_remain, "%ld", val);
+                } else if (*p == 'u') {
+                    unsigned long val = va_arg(args, unsigned long);
+                    written = snprintf(buf_ptr, buf_remain, "%lu", val);
+                } else if (*p == 'x' || *p == 'X') {
+                    unsigned long val = va_arg(args, unsigned long);
+                    written = snprintf(buf_ptr, buf_remain, *p == 'x' ? "%lx" : "%lX", val);
+                } else {
+                    p--;
+                }
+                break;
+            }
+            default: {
+                *buf_ptr++ = '%';
+                buf_remain--;
+                if (buf_remain > 0) {
+                    *buf_ptr++ = *p;
+                    buf_remain--;
+                }
+                written = 0;
+                break;
+            }
+        }
+        
+        if (written > 0) {
+            buf_ptr += written;
+            buf_remain -= (size_t)written;
+        }
+        p++;
+    }
+    
+    *buf_ptr = '\0';
+    return (int)(buf_ptr - buf);
+}
+
+int MRobot_Snprintf(char *buf, size_t size, const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    int len = format_float_va(buf, size, fmt, args);
+    va_end(args);
+    return len;
+}
+
+int MRobot_FormatFloat(char *buf, size_t size, float val, int precision) {
+    if (buf == NULL || size == 0) return 0;
+    
+    int offset = 0;
+    
+    /* 处理负数 */
+    if (val < 0) {
+        if (offset < (int)size - 1) buf[offset++] = '-';
+        val = -val;
+    }
+    
+    /* 限制精度范围 */
+    if (precision < 0) precision = 0;
+    if (precision > 6) precision = 6;
+    
+    /* 计算乘数 */
+    int multiplier = 1;
+    for (int i = 0; i < precision; i++) multiplier *= 10;
+    
+    int int_part = (int)val;
+    int frac_part = (int)((val - int_part) * multiplier + 0.5f);
+    
+    /* 处理进位 */
+    if (frac_part >= multiplier) {
+        int_part++;
+        frac_part -= multiplier;
+    }
+    
+    /* 格式化输出 */
+    int written;
+    if (precision > 0) {
+        written = snprintf(buf + offset, size - offset, "%d.%0*d", int_part, precision, frac_part);
+    } else {
+        written = snprintf(buf + offset, size - offset, "%d", int_part);
+    }
+    return (written > 0) ? (offset + written) : offset;
+}
+
+void MRobot_Run(void) {
+    if (!ctx.initialized) return;
+    
+    /* htop 模式 */
+    if (ctx.state == MROBOT_STATE_HTOP) {
+        handle_htop_mode();
+        return;
+    }
+    
+    /* 处理命令 */
+    if (ctx.cmd_ready) {
+        ctx.state = MROBOT_STATE_PROCESSING;
+        
+        /* 检查是否是 htop 命令 */
+        if (strcmp((char *)ctx.cmd_buffer, "htop") == 0) {
+            ctx.state = MROBOT_STATE_HTOP;
+            ctx.htop_exit = false;
+        } else {
+            /* 处理其他命令 */
+            BaseType_t more;
+            do {
+                ctx.output_buffer[0] = '\0';
+                more = FreeRTOS_CLIProcessCommand((char *)ctx.cmd_buffer,
+                                                   ctx.output_buffer,
+                                                   sizeof(ctx.output_buffer));
+                
+                if (ctx.output_buffer[0] != '\0') {
+                    send_string(ctx.output_buffer);
+                }
+            } while (more != pdFALSE);
+            
+            send_prompt();
+            ctx.state = MROBOT_STATE_IDLE;
+        }
+        
+        ctx.cmd_index = 0;
+        ctx.cmd_ready = false;
+    }
+    
+    osDelay(10);
+}

User/device/mrobot.h

@@ -0,0 +1,325 @@
+/**
+ * @file mrobot.h
+ * @brief MRobot CLI - 基于 FreeRTOS CLI 的嵌入式调试命令行系统
+ * 
+ * 功能特性:
+ * - 设备注册与监控（IMU、电机、传感器等）
+ * - 类 Unix 文件系统命令（cd, ls, pwd）
+ * - htop 风格的任务监控
+ * - 自定义命令扩展
+ * - 线程安全设计
+ * 
+ * @example IMU 设备注册示例
+ * @code
+ * // 1. 定义 IMU 数据结构
+ * typedef struct {
+ *     bool online;
+ *     float accl[3];
+ *     float gyro[3];
+ *     float euler[3];  // roll, pitch, yaw (deg)
+ *     float temp;
+ * } MyIMU_t;
+ * 
+ * MyIMU_t my_imu;
+ * 
+ * // 2. 实现打印回调
+ * static int print_imu(const void *data, char *buf, size_t size) {
+ *     const MyIMU_t *imu = (const MyIMU_t *)data;
+ *     char ax[16], ay[16], az[16], r[16], p[16], y[16], t[16];
+ *     MRobot_FormatFloat(ax, 16, imu->accl[0], 3);
+ *     MRobot_FormatFloat(ay, 16, imu->accl[1], 3);
+ *     MRobot_FormatFloat(az, 16, imu->accl[2], 3);
+ *     MRobot_FormatFloat(r, 16, imu->euler[0], 2);
+ *     MRobot_FormatFloat(p, 16, imu->euler[1], 2);
+ *     MRobot_FormatFloat(y, 16, imu->euler[2], 2);
+ *     MRobot_FormatFloat(t, 16, imu->temp, 1);
+ *     return snprintf(buf, size,
+ *         "  Status: %s\r\n"
+ *         "  Accel : X=%s Y=%s Z=%s\r\n"
+ *         "  Euler : R=%s P=%s Y=%s\r\n"
+ *         "  Temp  : %s C\r\n",
+ *         imu->online ? "Online" : "Offline", ax, ay, az, r, p, y, t);
+ * }
+ * 
+ * // 3. 注册设备
+ * MRobot_RegisterDevice("imu", &my_imu, print_imu);
+ * @endcode
+ * 
+ * @example 电机设备注册示例
+ * @code
+ * typedef struct {
+ *     bool online;
+ *     float angle;    // deg
+ *     float speed;    // RPM
+ *     float current;  // A
+ * } MyMotor_t;
+ * 
+ * MyMotor_t motor[4];
+ * 
+ * static int print_motor(const void *data, char *buf, size_t size) {
+ *     const MyMotor_t *m = (const MyMotor_t *)data;
+ *     char ang[16], spd[16], cur[16];
+ *     MRobot_FormatFloat(ang, 16, m->angle, 2);
+ *     MRobot_FormatFloat(spd, 16, m->speed, 1);
+ *     MRobot_FormatFloat(cur, 16, m->current, 3);
+ *     return snprintf(buf, size,
+ *         "  Status : %s\r\n"
+ *         "  Angle  : %s deg\r\n"
+ *         "  Speed  : %s RPM\r\n"
+ *         "  Current: %s A\r\n",
+ *         m->online ? "Online" : "Offline", ang, spd, cur);
+ * }
+ * 
+ * // 注册 4 个电机
+ * MRobot_RegisterDevice("motor0", &motor[0], print_motor);
+ * MRobot_RegisterDevice("motor1", &motor[1], print_motor);
+ * MRobot_RegisterDevice("motor2", &motor[2], print_motor);
+ * MRobot_RegisterDevice("motor3", &motor[3], print_motor);
+ * @endcode
+ * 
+ * @example 自定义校准命令示例
+ * @code
+ * // 校准数据
+ * static float gyro_offset[3] = {0};
+ * 
+ * // 命令回调: cali gyro / cali accel / cali save
+ * static long cmd_cali(char *buf, size_t size, const char *cmd) {
+ *     const char *param = FreeRTOS_CLIGetParameter(cmd, 1, NULL);
+ *     
+ *     if (param == NULL) {
+ *         return snprintf(buf, size, "Usage: cali <gyro|accel|save>\r\n");
+ *     }
+ *     if (strncmp(param, "gyro", 4) == 0) {
+ *         // 采集 1000 次陀螺仪数据求平均
+ *         snprintf(buf, size, "Calibrating gyro... keep IMU still!\r\n");
+ *         // ... 校准逻辑 ...
+ *         return 0;
+ *     }
+ *     if (strncmp(param, "save", 4) == 0) {
+ *         // 保存到 Flash
+ *         snprintf(buf, size, "Calibration saved to flash.\r\n");
+ *         return 0;
+ *     }
+ *     return snprintf(buf, size, "Unknown: %s\r\n", param);
+ * }
+ * 
+ * // 注册命令
+ * MRobot_RegisterCommand("cali", "cali <gyro|accel|save>: IMU calibration\r\n", cmd_cali, -1);
+ * @endcode
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include "bsp/uart.h"
+
+/* Configuration ------------------------------------------------------------ */
+/* 可在编译时通过 -D 选项覆盖这些默认值 */
+
+#ifndef MROBOT_MAX_DEVICES
+#define MROBOT_MAX_DEVICES          64      /* 最大设备数 */
+#endif
+
+#ifndef MROBOT_MAX_CUSTOM_COMMANDS
+#define MROBOT_MAX_CUSTOM_COMMANDS  16      /* 最大自定义命令数 */
+#endif
+
+#ifndef MROBOT_CMD_BUFFER_SIZE
+#define MROBOT_CMD_BUFFER_SIZE      128     /* 命令缓冲区大小 */
+#endif
+
+#ifndef MROBOT_OUTPUT_BUFFER_SIZE
+#define MROBOT_OUTPUT_BUFFER_SIZE   512     /* 输出缓冲区大小 */
+#endif
+
+#ifndef MROBOT_DEVICE_NAME_LEN
+#define MROBOT_DEVICE_NAME_LEN      32      /* 设备名最大长度 */
+#endif
+
+#ifndef MROBOT_PATH_MAX_LEN
+#define MROBOT_PATH_MAX_LEN         64      /* 路径最大长度 */
+#endif
+
+#ifndef MROBOT_HTOP_REFRESH_MS
+#define MROBOT_HTOP_REFRESH_MS      200     /* htop 刷新间隔 (ms) */
+#endif
+
+#ifndef MROBOT_UART_PORT
+#define MROBOT_UART_PORT            BSP_UART_VOFA  /* 默认 UART 端口 */
+#endif
+
+#ifndef MROBOT_USER_NAME
+#define MROBOT_USER_NAME            "root"         /* CLI 用户名 */
+#endif
+
+#ifndef MROBOT_HOST_NAME
+#define MROBOT_HOST_NAME            "MRobot"       /* CLI 主机名 */
+#endif
+
+/* Error codes -------------------------------------------------------------- */
+typedef enum {
+    MROBOT_OK               =  0,   /* 成功 */
+    MROBOT_ERR_FULL         = -1,   /* 容量已满 */
+    MROBOT_ERR_NULL_PTR     = -2,   /* 空指针 */
+    MROBOT_ERR_INVALID_ARG  = -3,   /* 无效参数 */
+    MROBOT_ERR_NOT_FOUND    = -4,   /* 未找到 */
+    MROBOT_ERR_ALLOC        = -5,   /* 内存分配失败 */
+    MROBOT_ERR_BUSY         = -6,   /* 设备忙 */
+    MROBOT_ERR_NOT_INIT     = -7,   /* 未初始化 */
+} MRobot_Error_t;
+
+/* CLI 运行状态 */
+typedef enum {
+    MROBOT_STATE_IDLE,              /* 空闲状态，等待输入 */
+    MROBOT_STATE_HTOP,              /* htop 模式 */
+    MROBOT_STATE_PROCESSING,        /* 正在处理命令 */
+} MRobot_State_t;
+
+/* Callback types ----------------------------------------------------------- */
+
+/**
+ * @brief 设备打印回调函数类型
+ * @param device_data 用户注册时传入的设备数据指针
+ * @param buffer 输出缓冲区
+ * @param buffer_size 缓冲区大小
+ * @return 实际写入的字节数
+ * @note 用户需要自行实现此函数来格式化设备数据
+ */
+typedef int (*MRobot_PrintCallback_t)(const void *device_data, char *buffer, size_t buffer_size);
+
+/**
+ * @brief 命令处理回调函数类型（与 FreeRTOS CLI 兼容）
+ */
+typedef long (*MRobot_CommandCallback_t)(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
+
+/* Device structure --------------------------------------------------------- */
+typedef struct {
+    char name[MROBOT_DEVICE_NAME_LEN];  /* 设备名称 */
+    void *data;                         /* 用户设备数据指针 */
+    MRobot_PrintCallback_t print_cb;    /* 用户打印回调函数 */
+} MRobot_Device_t;
+
+/* Public API --------------------------------------------------------------- */
+
+/**
+ * @brief 初始化 MRobot CLI 系统
+ * @note 必须在 FreeRTOS 调度器启动后调用
+ */
+void MRobot_Init(void);
+
+/**
+ * @brief 反初始化 MRobot CLI 系统，释放资源
+ */
+void MRobot_DeInit(void);
+
+/**
+ * @brief 获取当前 CLI 状态
+ * @return MRobot_State_t 当前状态
+ */
+MRobot_State_t MRobot_GetState(void);
+
+/**
+ * @brief 注册设备到 MRobot 系统
+ * @param name 设备名称（会被截断到 MROBOT_DEVICE_NAME_LEN-1）
+ * @param data 设备数据指针（不能为 NULL）
+ * @param print_cb 打印回调函数（可为 NULL，但无法用 show 命令查看）
+ * @return MRobot_Error_t 错误码
+ */
+MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb);
+
+/**
+ * @brief 注销设备
+ * @param name 设备名称
+ * @return MRobot_Error_t 错误码
+ */
+MRobot_Error_t MRobot_UnregisterDevice(const char *name);
+
+/**
+ * @brief 注册自定义命令
+ * @param command 命令名称
+ * @param help_text 帮助文本
+ * @param callback 命令回调函数
+ * @param param_count 参数个数（-1 表示可变参数）
+ * @return MRobot_Error_t 错误码
+ */
+MRobot_Error_t MRobot_RegisterCommand(const char *command, const char *help_text,
+                                       MRobot_CommandCallback_t callback, int8_t param_count);
+
+/**
+ * @brief 获取已注册设备数量
+ * @return 设备数量
+ */
+uint8_t MRobot_GetDeviceCount(void);
+
+/**
+ * @brief 根据名称查找设备
+ * @param name 设备名称
+ * @return 设备指针，未找到返回 NULL
+ */
+const MRobot_Device_t *MRobot_FindDevice(const char *name);
+
+/**
+ * @brief MRobot 主循环，在 CLI 任务中周期性调用
+ * @note 建议调用周期 10ms
+ */
+void MRobot_Run(void);
+
+/**
+ * @brief 格式化输出到 CLI 终端（线程安全，支持浮点数）
+ * @param fmt 格式字符串
+ * @param ... 可变参数
+ * @return 实际输出的字符数，失败返回负数
+ * 
+ * @note 支持的格式说明符:
+ *   - %d, %i, %u, %x, %X, %ld, %lu, %lx  : 整数
+ *   - %s, %c   : 字符串/字符
+ *   - %f       : 浮点数 (默认2位小数)
+ *   - %.Nf     : 浮点数 (N位小数, N=0-6)
+ *   - %%       : 百分号
+ * 
+ * @example
+ *   MRobot_Printf("Euler: R=%.2f P=%.2f Y=%.2f\\r\\n", roll, pitch, yaw);
+ */
+int MRobot_Printf(const char *fmt, ...);
+
+/**
+ * @brief 格式化到缓冲区（用于回调函数，支持浮点数）
+ * @note 格式说明符同 MRobot_Printf
+ * 
+ * @example
+ *   static int print_imu(const void *data, char *buf, size_t size) {
+ *       const BMI088_t *imu = (const BMI088_t *)data;
+ *       return MRobot_Snprintf(buf, size,
+ *           "  Accel: X=%.3f Y=%.3f Z=%.3f\\r\\n",
+ *           imu->accl.x, imu->accl.y, imu->accl.z);
+ *   }
+ */
+int MRobot_Snprintf(char *buf, size_t size, const char *fmt, ...);
+
+/* Utility functions -------------------------------------------------------- */
+
+/**
+ * @brief 格式化浮点数为字符串（嵌入式环境不支持 %f）
+ * @param buf 输出缓冲区
+ * @param size 缓冲区大小
+ * @param val 要格式化的浮点数
+ * @param precision 小数位数 (0-6)
+ * @return 写入的字符数
+ * 
+ * @example
+ *   char buf[16];
+ *   MRobot_FormatFloat(buf, sizeof(buf), 3.14159f, 2);  // "3.14"
+ *   MRobot_FormatFloat(buf, sizeof(buf), -0.001f, 4);   // "-0.0010"
+ */
+int MRobot_FormatFloat(char *buf, size_t size, float val, int precision);
+
+#ifdef __cplusplus
+}
+#endif

User/device/vofa.c

@@ -0,0 +1,106 @@
+/* Includes ----------------------------------------------------------------- */
+#include <stdio.h>
+#include <string.h>
+#include "device/vofa.h"
+#include "bsp/uart.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+
+#define MAX_CHANNEL  64u   		// 根据实际最大通道数调整
+
+#define JUSTFLOAT_TAIL 0x7F800000
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)];
+static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER;  // 默认协议
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/************************  RawData  *************************/
+void VOFA_RawData_Send(const char* data, bool dma) {
+    BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t*)data, strlen(data), dma);
+}
+
+/************************ FireWater *************************/
+void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma)
+{
+    if (channel_count == 0 || channel_count > MAX_CHANNEL)
+        return;
+
+    char *buf = (char *)vofa_tx_buf;
+    size_t len = 0;
+
+    for (uint8_t i = 0; i < channel_count; ++i) {
+        len += snprintf(buf + len,
+                        sizeof(vofa_tx_buf) - len,
+                        "%s%.2f",
+                        (i ? "," : ""),
+                        channels[i]);
+    }
+    snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n");
+
+    BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, strlen(buf), dma);
+}
+
+/************************ JustFloat *************************/
+void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma)
+{
+    if (channel_count == 0 || channel_count > MAX_CHANNEL)
+        return;
+    memcpy(vofa_tx_buf, channels, channel_count * sizeof(float));
+
+    uint32_t tail = JUSTFLOAT_TAIL;                     // 0x7F800000
+    memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail));
+
+    BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t VOFA_init(VOFA_Protocol_t protocol) {
+    current_protocol = protocol;
+    return DEVICE_OK;
+}
+
+int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) {
+    switch (current_protocol) {
+        case VOFA_PROTOCOL_RAWDATA:
+            {
+                char data[256];
+                if (channel_count >= 1) {
+                    sprintf(data, "Channel1: %.2f", channels[0]);
+                    if (channel_count >= 2) {
+                        sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]);
+                    }
+                    strcat(data, "\n");
+                    VOFA_RawData_Send(data, dma);
+                }
+            }
+            break;
+        case VOFA_PROTOCOL_FIREWATER:
+            VOFA_FireWater_Send(channels, channel_count, dma);
+            break;
+        case VOFA_PROTOCOL_JUSTFLOAT:
+            VOFA_JustFloat_Send(channels, channel_count, dma);
+            break;
+        default:
+            return DEVICE_ERR;
+    }
+    return DEVICE_OK;
+}

User/device/vofa.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/uart.h"
+#include "device/device.h"
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+
+typedef enum {
+    VOFA_PROTOCOL_RAWDATA,
+    VOFA_PROTOCOL_FIREWATER,
+    VOFA_PROTOCOL_JUSTFLOAT,
+} VOFA_Protocol_t;
+
+/**
+ * @brief 初始化VOFA设备
+ * @param protocol 设置通信协议
+ * @return 
+ */
+int8_t VOFA_init(VOFA_Protocol_t protocol);
+
+/**
+ * @brief 发送数据到VOFA
+ * @param channels 要发送的通道数据
+ * @param channel_count 通道数量
+ * @param dma 是否使用DMA发送
+ * @return 
+ */
+int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma);
+
+#ifdef __cplusplus
+}
+#endif

User/module/shoot.c

@@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
 	
     memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
     memset(&s->output,0,sizeof(s->output));
-    s->target_variable.target_rpm=6000.0f;  /* 归一化目标转速 */
+    s->target_variable.target_rpm=2000.0f;  /* 归一化目标转速 */
 
 	return 0;
 }

User/task/ai.c

@@ -4,7 +4,6 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "bsp/fdcan.h"

User/task/atti_esit.c

@@ -10,10 +10,14 @@
 #include "bsp/mm.h"
 #include "bsp/pwm.h"
 #include "component/ahrs.h"
+#include "component/QuaternionEKF.h"
 #include "component/pid.h"
 #include "device/bmi088.h"
+#include "device/mrobot.h"
+#include "device/device.h"
 #include "module/balance_chassis.h"
 #include "task/user_task.h"
+#include <stdio.h>
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -22,9 +26,10 @@
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
 BMI088_t bmi088;
-
+#ifndef USE_QEKF
 AHRS_t gimbal_ahrs;
 AHRS_Magn_t magn;
+#endif
 AHRS_Eulr_t eulr_to_send;
 Chassis_IMU_t imu_for_chassis;
 KPID_t imu_temp_ctrl_pid;
@@ -46,6 +51,27 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = {
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
+
+/**
+ * @brief IMU 设备打印回调函数
+ */
+static int print_imu_callback(const void *data, char *buffer, size_t size) {
+    const BMI088_t *imu = (const BMI088_t *)data;
+    #define RAD2DEG 57.2957795131f
+    
+    return MRobot_Snprintf(buffer, size,
+        "  Status : %s\r\n"
+        "  Accel  : X=%.3f  Y=%.3f  Z=%.3f (m/s^2)\r\n"
+        "  Gyro   : X=%.3f  Y=%.3f  Z=%.3f (rad/s)\r\n"
+        "  Euler  : R=%.2f  P=%.2f  Y=%.2f (deg)\r\n"
+        "  Temp   : %.1f C\r\n",
+        imu->header.online ? "Online" : "Offline",
+        imu->accl.x, imu->accl.y, imu->accl.z,
+        imu->gyro.x, imu->gyro.y, imu->gyro.z,
+        eulr_to_send.rol * RAD2DEG, eulr_to_send.pit * RAD2DEG, eulr_to_send.yaw * RAD2DEG,
+        imu->temp);
+}
+
 /* Exported functions ------------------------------------------------------- */
 void Task_atti_esit(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
@@ -55,10 +81,17 @@ void Task_atti_esit(void *argument) {
 
   /* USER CODE INIT BEGIN */
   BMI088_Init(&bmi088, &cali_bmi088);
+#ifdef USE_QEKF
+  QEKF_Init(10.0f, 0.001f, 1000000.0f, 0.9996f, 0.0f);
+#else
   AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
+#endif
   PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
            1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
   BSP_PWM_Start(BSP_PWM_IMU_HEAT);
+  
+  /* 注册 IMU 设备到 MRobot */
+  MRobot_RegisterDevice("bmi088", &bmi088, print_imu_callback);
   /* USER CODE INIT END */
 
   while (1) {
@@ -78,10 +111,19 @@ void Task_atti_esit(void *argument) {
     // IST8310_Parse(&ist8310);
 
     /* 根据设备接收到的数据进行姿态解析 */
+#ifdef USE_QEKF
+    QEKF_Update(bmi088.gyro.x, bmi088.gyro.y, bmi088.gyro.z,
+                bmi088.accl.x, bmi088.accl.y, bmi088.accl.z,
+                1.0f / BMI088_GetUpdateFreq(&bmi088));
+    /* 从EKF获取欧拉角 */
+    eulr_to_send.rol = QEKF_INS.roll;
+    eulr_to_send.pit = QEKF_INS.pitch;
+    eulr_to_send.yaw = QEKF_INS.yaw;
+#else
     AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
-
     /* 根据解析出来的四元数计算欧拉角 */
     AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+#endif
     /* 加速度计数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
     imu_for_chassis.accl.x = bmi088.accl.y;
     imu_for_chassis.accl.y = -bmi088.accl.x;

User/task/cli.c

@@ -0,0 +1,40 @@
+/*
+    cli Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "device/mrobot.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_cli(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  osDelay(CLI_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  /* USER CODE INIT BEGIN */
+  /* 初始化 MRobot CLI 系统 */
+  MRobot_Init();
+  /* USER CODE INIT END */
+
+  while (1) {
+    /* USER CODE BEGIN */
+    /* 运行 MRobot 主循环 */
+    MRobot_Run();
+    /* USER CODE END */
+  }
+  
+}

User/task/config.yaml

@@ -53,3 +53,16 @@
   function: Task_ai
   name: ai
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_vofa
+  name: vofa
+  stack: 256
+- delay: 0
+  description: ''
+  freq_control: false
+  function: Task_cli
+  name: cli
+  stack: 1024

User/task/ctrl_chassis.c

@@ -69,6 +69,8 @@ void Task_ctrl_chassis(void *argument) {
     }
 
     Chassis_Control(&chassis, &chassis_cmd);  
+    osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞
+    osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/ctrl_shoot.c

@@ -24,7 +24,6 @@ Shoot_CMD_t shoot_cmd;
 void Task_ctrl_shoot(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
   
-  
   /* 计算任务运行到指定频率需要等待的tick数 */
   const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;
 

User/task/init.c

@@ -40,6 +40,8 @@ void Task_Init(void *argument) {
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
   task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
   task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
+  // task_runtime.thread.vofa = osThreadNew(Task_vofa, NULL, &attr_vofa);
+  task_runtime.thread.cli = osThreadNew(Task_cli, NULL, &attr_cli);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
@@ -48,8 +50,10 @@ void Task_Init(void *argument) {
   task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
   task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
   task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
+  task_runtime.msgq.chassis.vofa = osMessageQueueNew(2u, sizeof(Chassis_t), NULL);
   task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
   task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
+  
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/user_task.c

@@ -49,3 +49,13 @@ const osThreadAttr_t attr_ai = {
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
 };
+const osThreadAttr_t attr_vofa = {
+    .name = "vofa",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_cli = {
+    .name = "cli",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 16,
+};

User/task/user_task.h

@@ -20,6 +20,7 @@ extern "C" {
 #define BLINK_FREQ (500.0)
 #define CTRL_SHOOT_FREQ (500.0)
 #define AI_FREQ (500.0)
+#define VOFA_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -31,6 +32,8 @@ extern "C" {
 #define BLINK_INIT_DELAY (0)
 #define CTRL_SHOOT_INIT_DELAY (0)
 #define AI_INIT_DELAY (0)
+#define VOFA_INIT_DELAY (0)
+#define CLI_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -48,6 +51,8 @@ typedef struct {
         osThreadId_t blink;
         osThreadId_t ctrl_shoot;
         osThreadId_t ai;
+        osThreadId_t vofa;
+        osThreadId_t cli;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -57,6 +62,7 @@ typedef struct {
             osMessageQueueId_t imu;
             osMessageQueueId_t cmd;
             osMessageQueueId_t yaw;
+            osMessageQueueId_t vofa;
         }chassis;
         struct {
             osMessageQueueId_t imu;
@@ -101,6 +107,8 @@ typedef struct {
         UBaseType_t blink;
         UBaseType_t ctrl_shoot;
         UBaseType_t ai;
+        UBaseType_t vofa;
+        UBaseType_t cli;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -112,6 +120,7 @@ typedef struct {
         float blink;
         float ctrl_shoot;
         float ai;
+        float vofa;
     } freq;
 
     /* 任务最近运行时间 */
@@ -123,6 +132,7 @@ typedef struct {
         float blink;
         float ctrl_shoot;
         float ai;
+        float vofa;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -140,6 +150,8 @@ extern const osThreadAttr_t attr_monitor;
 extern const osThreadAttr_t attr_blink;
 extern const osThreadAttr_t attr_ctrl_shoot;
 extern const osThreadAttr_t attr_ai;
+extern const osThreadAttr_t attr_vofa;
+extern const osThreadAttr_t attr_cli;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -151,6 +163,8 @@ void Task_monitor(void *argument);
 void Task_blink(void *argument);
 void Task_ctrl_shoot(void *argument);
 void Task_ai(void *argument);
+void Task_vofa(void *argument);
+void Task_cli(void *argument);
 
 #ifdef __cplusplus
 }

User/task/vofa.c

@@ -0,0 +1,84 @@
+/*
+    vofa Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "module/balance_chassis.h"
+#include "device/vofa.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+Chassis_t chassis_vofa;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_vofa(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / VOFA_FREQ;
+
+  osDelay(VOFA_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    // 从消息队列获取chassis数据
+    if (osMessageQueueGet(task_runtime.msgq.chassis.vofa, &chassis_vofa, NULL, 0) == osOK) {
+      // 准备要发送的数据通道
+      float channels[16];
+      
+      // 通道0-1: 左右腿的VMC腿长
+      channels[0] = chassis_vofa.vmc_[0].leg.L0;
+      channels[1] = chassis_vofa.vmc_[1].leg.L0;
+      
+      // 通道2-3: 左右腿的VMC角度
+      channels[2] = chassis_vofa.vmc_[0].leg.theta;
+      channels[3] = chassis_vofa.vmc_[1].leg.theta;
+      
+      // 通道4-6: IMU欧拉角 (pitch, roll, yaw)
+      channels[4] = chassis_vofa.feedback.imu.euler.pit;
+      channels[5] = chassis_vofa.feedback.imu.euler.rol;
+      channels[6] = chassis_vofa.feedback.imu.euler.yaw;
+      
+      // 通道7-8: 左右轮速度
+      channels[7] = chassis_vofa.vmc_[0].leg.d_theta;
+      channels[8] = chassis_vofa.vmc_[0].leg.dd_theta;
+      
+      // 通道9-10: 机体位置和速度
+      channels[9] = chassis_vofa.chassis_state.position_x;
+      channels[10] = chassis_vofa.chassis_state.velocity_x;
+      
+      // 通道11-12: 左右腿的虚拟支撑力
+      channels[11] = chassis_vofa.vmc_[0].leg.F_virtual;
+      channels[12] = chassis_vofa.vmc_[1].leg.F_virtual;
+      
+      // 通道13-14: 左右腿的虚拟摆力矩
+      channels[13] = chassis_vofa.vmc_[0].leg.T_virtual;
+      channels[14] = chassis_vofa.vmc_[1].leg.T_virtual;
+      
+      // 通道15: yaw控制力矩
+      channels[15] = chassis_vofa.yaw_control.yaw_force;
+      
+      // 使用JustFloat协议和DMA发送
+      VOFA_Send(channels, 16, true);
+    }
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

cmake/stm32cubemx/CMakeLists.txt

@@ -19,6 +19,7 @@ set(MX_Include_Dirs
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32H7xx/Include
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/ARM/DSP/Inc
 )
 
 # STM32CubeMX generated application sources
@@ -27,6 +28,7 @@ set(MX_Application_Src
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/adc.c
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/bdma.c
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/dma.c
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/fdcan.c
     ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/octospi.c