CMakeLists.txt

@@ -56,14 +56,12 @@ 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
@@ -81,8 +79,6 @@ 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
@@ -100,9 +96,7 @@ 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
@@ -122,7 +116,6 @@ 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,10 +51,6 @@
 #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"
@@ -72,11 +68,9 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)0x20000)
+#define configTOTAL_HEAP_SIZE                    ((size_t)0x10000)
 #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
@@ -169,12 +163,6 @@ 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,14 +34,11 @@ 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

@@ -1,52 +0,0 @@
-/* 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,8 +56,6 @@ 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);
@@ -69,8 +67,6 @@ 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,9 +25,7 @@
 /* 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)
@@ -101,75 +99,36 @@ 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();
 
@@ -212,39 +171,6 @@ 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)
@@ -272,23 +198,6 @@ 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

@@ -1,55 +0,0 @@
-/* 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,12 +55,6 @@ 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,23 +65,6 @@ 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,7 +20,6 @@
 #include "main.h"
 #include "cmsis_os.h"
 #include "adc.h"
-#include "bdma.h"
 #include "dma.h"
 #include "fdcan.h"
 #include "octospi.h"
@@ -58,7 +57,6 @@
 
 /* Private function prototypes -----------------------------------------------*/
 void SystemClock_Config(void);
-void PeriphCommonClock_Config(void);
 void MX_FREERTOS_Init(void);
 /* USER CODE BEGIN PFP */
 
@@ -92,9 +90,6 @@ int main(void)
   /* Configure the system clock */
   SystemClock_Config();
 
-  /* Configure the peripherals common clocks */
-  PeriphCommonClock_Config();
-
   /* USER CODE BEGIN SysInit */
 
   /* USER CODE END SysInit */
@@ -102,7 +97,6 @@ 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();
@@ -119,9 +113,8 @@ int main(void)
   MX_TIM1_Init();
   MX_TIM2_Init();
   MX_OCTOSPI1_Init();
-  MX_UART5_Init();
-  MX_ADC3_Init();
   MX_USB_OTG_HS_PCD_Init();
+  MX_UART5_Init();
   /* USER CODE BEGIN 2 */
 
   /* USER CODE END 2 */
@@ -209,32 +202,6 @@ 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

@@ -1,508 +1,445 @@
-/* USER CODE BEGIN Header */
+/* USER CODE BEGIN Header */
-/**
+/**
-  ******************************************************************************
+  ******************************************************************************
-  * @file    stm32h7xx_it.c
+  * @file    stm32h7xx_it.c
-  * @brief   Interrupt Service Routines.
+  * @brief   Interrupt Service Routines.
-  ******************************************************************************
+  ******************************************************************************
-  * @attention
+  * @attention
-  *
+  *
-  * Copyright (c) 2026 STMicroelectronics.
+  * Copyright (c) 2026 STMicroelectronics.
-  * All rights reserved.
+  * All rights reserved.
-  *
+  *
-  * This software is licensed under terms that can be found in the LICENSE file
+  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
+  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
+  *
-  ******************************************************************************
+  ******************************************************************************
-  */
+  */
-/* USER CODE END Header */
+/* USER CODE END Header */
-
+
-/* Includes ------------------------------------------------------------------*/
+/* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "main.h"
-#include "stm32h7xx_it.h"
+#include "stm32h7xx_it.h"
-/* Private includes ----------------------------------------------------------*/
+/* Private includes ----------------------------------------------------------*/
-/* USER CODE BEGIN Includes */
+/* USER CODE BEGIN Includes */
-#include "bsp/uart.h"
+/* USER CODE END Includes */
-/* USER CODE END Includes */
+
-
+/* Private typedef -----------------------------------------------------------*/
-/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN TD */
-/* USER CODE BEGIN TD */
+
-
+/* USER CODE END TD */
-/* USER CODE END TD */
+
-
+/* Private define ------------------------------------------------------------*/
-/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN PD */
-/* USER CODE BEGIN PD */
+
-
+/* USER CODE END PD */
-/* USER CODE END PD */
+
-
+/* Private macro -------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN PM */
-/* USER CODE BEGIN PM */
+
-
+/* USER CODE END PM */
-/* USER CODE END PM */
+
-
+/* Private variables ---------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
+/* USER CODE BEGIN PV */
-/* USER CODE BEGIN PV */
+
-
+/* USER CODE END PV */
-/* USER CODE END PV */
+
-
+/* Private function prototypes -----------------------------------------------*/
-/* Private function prototypes -----------------------------------------------*/
+/* USER CODE BEGIN PFP */
-/* USER CODE BEGIN PFP */
+
-
+/* USER CODE END PFP */
-/* USER CODE END PFP */
+
-
+/* Private user code ---------------------------------------------------------*/
-/* Private user code ---------------------------------------------------------*/
+/* USER CODE BEGIN 0 */
-/* USER CODE BEGIN 0 */
+
-
+/* USER CODE END 0 */
-/* USER CODE END 0 */
+
-
+/* External variables --------------------------------------------------------*/
-/* External variables --------------------------------------------------------*/
+extern DMA_HandleTypeDef hdma_adc1;
-extern DMA_HandleTypeDef hdma_adc1;
+extern FDCAN_HandleTypeDef hfdcan1;
-extern DMA_HandleTypeDef hdma_adc3;
+extern FDCAN_HandleTypeDef hfdcan2;
-extern ADC_HandleTypeDef hadc3;
+extern FDCAN_HandleTypeDef hfdcan3;
-extern FDCAN_HandleTypeDef hfdcan1;
+extern DMA_HandleTypeDef hdma_spi2_rx;
-extern FDCAN_HandleTypeDef hfdcan2;
+extern DMA_HandleTypeDef hdma_spi2_tx;
-extern FDCAN_HandleTypeDef hfdcan3;
+extern SPI_HandleTypeDef hspi2;
-extern DMA_HandleTypeDef hdma_spi2_rx;
+extern DMA_HandleTypeDef hdma_uart5_rx;
-extern DMA_HandleTypeDef hdma_spi2_tx;
+extern UART_HandleTypeDef huart5;
-extern SPI_HandleTypeDef hspi2;
+extern UART_HandleTypeDef huart7;
-extern DMA_HandleTypeDef hdma_uart5_rx;
+extern UART_HandleTypeDef huart1;
-extern DMA_HandleTypeDef hdma_usart1_tx;
+extern UART_HandleTypeDef huart2;
-extern DMA_HandleTypeDef hdma_usart1_rx;
+extern UART_HandleTypeDef huart3;
-extern UART_HandleTypeDef huart5;
+extern UART_HandleTypeDef huart10;
-extern UART_HandleTypeDef huart7;
+extern TIM_HandleTypeDef htim23;
-extern UART_HandleTypeDef huart1;
+
-extern UART_HandleTypeDef huart2;
+/* USER CODE BEGIN EV */
-extern UART_HandleTypeDef huart3;
+
-extern UART_HandleTypeDef huart10;
+/* USER CODE END EV */
-extern TIM_HandleTypeDef htim23;
+
-
+/******************************************************************************/
-/* USER CODE BEGIN EV */
+/*           Cortex Processor Interruption and Exception Handlers          */
-
+/******************************************************************************/
-/* USER CODE END EV */
+/**
-
+  * @brief This function handles Non maskable interrupt.
-/******************************************************************************/
+  */
-/*           Cortex Processor Interruption and Exception Handlers          */
+void NMI_Handler(void)
-/******************************************************************************/
+{
-/**
+  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
-  * @brief This function handles Non maskable interrupt.
+
-  */
+  /* USER CODE END NonMaskableInt_IRQn 0 */
-void NMI_Handler(void)
+  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
-{
+   while (1)
-  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
+  {
-
+  }
-  /* USER CODE END NonMaskableInt_IRQn 0 */
+  /* USER CODE END NonMaskableInt_IRQn 1 */
-  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
+}
-   while (1)
+
-  {
+/**
-  }
+  * @brief This function handles Hard fault interrupt.
-  /* USER CODE END NonMaskableInt_IRQn 1 */
+  */
-}
+void HardFault_Handler(void)
-
+{
-/**
+  /* USER CODE BEGIN HardFault_IRQn 0 */
-  * @brief This function handles Hard fault interrupt.
+
-  */
+  /* USER CODE END HardFault_IRQn 0 */
-void HardFault_Handler(void)
+  while (1)
-{
+  {
-  /* USER CODE BEGIN HardFault_IRQn 0 */
+    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
-
+    /* USER CODE END W1_HardFault_IRQn 0 */
-  /* USER CODE END HardFault_IRQn 0 */
+  }
-  while (1)
+}
-  {
+
-    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
+/**
-    /* USER CODE END W1_HardFault_IRQn 0 */
+  * @brief This function handles Memory management fault.
-  }
+  */
-}
+void MemManage_Handler(void)
-
+{
-/**
+  /* USER CODE BEGIN MemoryManagement_IRQn 0 */
-  * @brief This function handles Memory management fault.
+
-  */
+  /* USER CODE END MemoryManagement_IRQn 0 */
-void MemManage_Handler(void)
+  while (1)
-{
+  {
-  /* USER CODE BEGIN MemoryManagement_IRQn 0 */
+    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
-
+    /* USER CODE END W1_MemoryManagement_IRQn 0 */
-  /* USER CODE END MemoryManagement_IRQn 0 */
+  }
-  while (1)
+}
-  {
+
-    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
+/**
-    /* USER CODE END W1_MemoryManagement_IRQn 0 */
+  * @brief This function handles Pre-fetch fault, memory access fault.
-  }
+  */
-}
+void BusFault_Handler(void)
-
+{
-/**
+  /* USER CODE BEGIN BusFault_IRQn 0 */
-  * @brief This function handles Pre-fetch fault, memory access fault.
+
-  */
+  /* USER CODE END BusFault_IRQn 0 */
-void BusFault_Handler(void)
+  while (1)
-{
+  {
-  /* USER CODE BEGIN BusFault_IRQn 0 */
+    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
-
+    /* USER CODE END W1_BusFault_IRQn 0 */
-  /* USER CODE END BusFault_IRQn 0 */
+  }
-  while (1)
+}
-  {
+
-    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
+/**
-    /* USER CODE END W1_BusFault_IRQn 0 */
+  * @brief This function handles Undefined instruction or illegal state.
-  }
+  */
-}
+void UsageFault_Handler(void)
-
+{
-/**
+  /* USER CODE BEGIN UsageFault_IRQn 0 */
-  * @brief This function handles Undefined instruction or illegal state.
+
-  */
+  /* USER CODE END UsageFault_IRQn 0 */
-void UsageFault_Handler(void)
+  while (1)
-{
+  {
-  /* USER CODE BEGIN UsageFault_IRQn 0 */
+    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
-
+    /* USER CODE END W1_UsageFault_IRQn 0 */
-  /* USER CODE END UsageFault_IRQn 0 */
+  }
-  while (1)
+}
-  {
+
-    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
+/**
-    /* USER CODE END W1_UsageFault_IRQn 0 */
+  * @brief This function handles Debug monitor.
-  }
+  */
-}
+void DebugMon_Handler(void)
-
+{
-/**
+  /* USER CODE BEGIN DebugMonitor_IRQn 0 */
-  * @brief This function handles Debug monitor.
+
-  */
+  /* USER CODE END DebugMonitor_IRQn 0 */
-void DebugMon_Handler(void)
+  /* USER CODE BEGIN DebugMonitor_IRQn 1 */
-{
+
-  /* USER CODE BEGIN DebugMonitor_IRQn 0 */
+  /* USER CODE END DebugMonitor_IRQn 1 */
-
+}
-  /* USER CODE END DebugMonitor_IRQn 0 */
+
-  /* USER CODE BEGIN DebugMonitor_IRQn 1 */
+/******************************************************************************/
-
+/* STM32H7xx Peripheral Interrupt Handlers                                    */
-  /* USER CODE END DebugMonitor_IRQn 1 */
+/* Add here the Interrupt Handlers for the used peripherals.                  */
-}
+/* For the available peripheral interrupt handler names,                      */
-
+/* please refer to the startup file (startup_stm32h7xx.s).                    */
-/******************************************************************************/
+/******************************************************************************/
-/* STM32H7xx Peripheral Interrupt Handlers                                    */
+
-/* Add here the Interrupt Handlers for the used peripherals.                  */
+/**
-/* For the available peripheral interrupt handler names,                      */
+  * @brief This function handles DMA1 stream0 global interrupt.
-/* please refer to the startup file (startup_stm32h7xx.s).                    */
+  */
-/******************************************************************************/
+void DMA1_Stream0_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN DMA1_Stream0_IRQn 0 */
-  * @brief This function handles DMA1 stream0 global interrupt.
+
-  */
+  /* USER CODE END DMA1_Stream0_IRQn 0 */
-void DMA1_Stream0_IRQHandler(void)
+  HAL_DMA_IRQHandler(&hdma_adc1);
-{
+  /* USER CODE BEGIN DMA1_Stream0_IRQn 1 */
-  /* USER CODE BEGIN DMA1_Stream0_IRQn 0 */
+
-
+  /* USER CODE END DMA1_Stream0_IRQn 1 */
-  /* USER CODE END DMA1_Stream0_IRQn 0 */
+}
-  HAL_DMA_IRQHandler(&hdma_adc1);
+
-  /* USER CODE BEGIN DMA1_Stream0_IRQn 1 */
+/**
-
+  * @brief This function handles DMA1 stream1 global interrupt.
-  /* USER CODE END DMA1_Stream0_IRQn 1 */
+  */
-}
+void DMA1_Stream1_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
-  * @brief This function handles DMA1 stream1 global interrupt.
+
-  */
+  /* USER CODE END DMA1_Stream1_IRQn 0 */
-void DMA1_Stream1_IRQHandler(void)
+  HAL_DMA_IRQHandler(&hdma_spi2_rx);
-{
+  /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
-  /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
+
-
+  /* USER CODE END DMA1_Stream1_IRQn 1 */
-  /* USER CODE END DMA1_Stream1_IRQn 0 */
+}
-  HAL_DMA_IRQHandler(&hdma_spi2_rx);
+
-  /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
+/**
-
+  * @brief This function handles DMA1 stream2 global interrupt.
-  /* USER CODE END DMA1_Stream1_IRQn 1 */
+  */
-}
+void DMA1_Stream2_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */
-  * @brief This function handles DMA1 stream2 global interrupt.
+
-  */
+  /* USER CODE END DMA1_Stream2_IRQn 0 */
-void DMA1_Stream2_IRQHandler(void)
+  HAL_DMA_IRQHandler(&hdma_spi2_tx);
-{
+  /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */
-  /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */
+
-
+  /* USER CODE END DMA1_Stream2_IRQn 1 */
-  /* USER CODE END DMA1_Stream2_IRQn 0 */
+}
-  HAL_DMA_IRQHandler(&hdma_spi2_tx);
+
-  /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */
+/**
-
+  * @brief This function handles DMA1 stream3 global interrupt.
-  /* USER CODE END DMA1_Stream2_IRQn 1 */
+  */
-}
+void DMA1_Stream3_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN DMA1_Stream3_IRQn 0 */
-  * @brief This function handles DMA1 stream3 global interrupt.
+
-  */
+  /* USER CODE END DMA1_Stream3_IRQn 0 */
-void DMA1_Stream3_IRQHandler(void)
+  HAL_DMA_IRQHandler(&hdma_uart5_rx);
-{
+  /* USER CODE BEGIN DMA1_Stream3_IRQn 1 */
-  /* USER CODE BEGIN DMA1_Stream3_IRQn 0 */
+
-
+  /* USER CODE END DMA1_Stream3_IRQn 1 */
-  /* USER CODE END DMA1_Stream3_IRQn 0 */
+}
-  HAL_DMA_IRQHandler(&hdma_uart5_rx);
+
-  /* USER CODE BEGIN DMA1_Stream3_IRQn 1 */
+/**
-
+  * @brief This function handles FDCAN1 interrupt 0.
-  /* USER CODE END DMA1_Stream3_IRQn 1 */
+  */
-}
+void FDCAN1_IT0_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN FDCAN1_IT0_IRQn 0 */
-  * @brief This function handles DMA1 stream4 global interrupt.
+
-  */
+  /* USER CODE END FDCAN1_IT0_IRQn 0 */
-void DMA1_Stream4_IRQHandler(void)
+  HAL_FDCAN_IRQHandler(&hfdcan1);
-{
+  /* USER CODE BEGIN FDCAN1_IT0_IRQn 1 */
-  /* USER CODE BEGIN DMA1_Stream4_IRQn 0 */
+
-
+  /* USER CODE END FDCAN1_IT0_IRQn 1 */
-  /* USER CODE END DMA1_Stream4_IRQn 0 */
+}
-  HAL_DMA_IRQHandler(&hdma_usart1_tx);
+
-  /* USER CODE BEGIN DMA1_Stream4_IRQn 1 */
+/**
-
+  * @brief This function handles FDCAN2 interrupt 0.
-  /* USER CODE END DMA1_Stream4_IRQn 1 */
+  */
-}
+void FDCAN2_IT0_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN FDCAN2_IT0_IRQn 0 */
-  * @brief This function handles DMA1 stream5 global interrupt.
+
-  */
+  /* USER CODE END FDCAN2_IT0_IRQn 0 */
-void DMA1_Stream5_IRQHandler(void)
+  HAL_FDCAN_IRQHandler(&hfdcan2);
-{
+  /* USER CODE BEGIN FDCAN2_IT0_IRQn 1 */
-  /* USER CODE BEGIN DMA1_Stream5_IRQn 0 */
+
-
+  /* USER CODE END FDCAN2_IT0_IRQn 1 */
-  /* USER CODE END DMA1_Stream5_IRQn 0 */
+}
-  HAL_DMA_IRQHandler(&hdma_usart1_rx);
+
-  /* USER CODE BEGIN DMA1_Stream5_IRQn 1 */
+/**
-
+  * @brief This function handles FDCAN1 interrupt 1.
-  /* USER CODE END DMA1_Stream5_IRQn 1 */
+  */
-}
+void FDCAN1_IT1_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN FDCAN1_IT1_IRQn 0 */
-  * @brief This function handles FDCAN1 interrupt 0.
+
-  */
+  /* USER CODE END FDCAN1_IT1_IRQn 0 */
-void FDCAN1_IT0_IRQHandler(void)
+  HAL_FDCAN_IRQHandler(&hfdcan1);
-{
+  /* USER CODE BEGIN FDCAN1_IT1_IRQn 1 */
-  /* USER CODE BEGIN FDCAN1_IT0_IRQn 0 */
+
-
+  /* USER CODE END FDCAN1_IT1_IRQn 1 */
-  /* USER CODE END FDCAN1_IT0_IRQn 0 */
+}
-  HAL_FDCAN_IRQHandler(&hfdcan1);
+
-  /* USER CODE BEGIN FDCAN1_IT0_IRQn 1 */
+/**
-
+  * @brief This function handles FDCAN2 interrupt 1.
-  /* USER CODE END FDCAN1_IT0_IRQn 1 */
+  */
-}
+void FDCAN2_IT1_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN FDCAN2_IT1_IRQn 0 */
-  * @brief This function handles FDCAN2 interrupt 0.
+
-  */
+  /* USER CODE END FDCAN2_IT1_IRQn 0 */
-void FDCAN2_IT0_IRQHandler(void)
+  HAL_FDCAN_IRQHandler(&hfdcan2);
-{
+  /* USER CODE BEGIN FDCAN2_IT1_IRQn 1 */
-  /* USER CODE BEGIN FDCAN2_IT0_IRQn 0 */
+
-
+  /* USER CODE END FDCAN2_IT1_IRQn 1 */
-  /* USER CODE END FDCAN2_IT0_IRQn 0 */
+}
-  HAL_FDCAN_IRQHandler(&hfdcan2);
+
-  /* USER CODE BEGIN FDCAN2_IT0_IRQn 1 */
+/**
-
+  * @brief This function handles SPI2 global interrupt.
-  /* USER CODE END FDCAN2_IT0_IRQn 1 */
+  */
-}
+void SPI2_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN SPI2_IRQn 0 */
-  * @brief This function handles FDCAN1 interrupt 1.
+
-  */
+  /* USER CODE END SPI2_IRQn 0 */
-void FDCAN1_IT1_IRQHandler(void)
+  HAL_SPI_IRQHandler(&hspi2);
-{
+  /* USER CODE BEGIN SPI2_IRQn 1 */
-  /* USER CODE BEGIN FDCAN1_IT1_IRQn 0 */
+
-
+  /* USER CODE END SPI2_IRQn 1 */
-  /* USER CODE END FDCAN1_IT1_IRQn 0 */
+}
-  HAL_FDCAN_IRQHandler(&hfdcan1);
+
-  /* USER CODE BEGIN FDCAN1_IT1_IRQn 1 */
+/**
-
+  * @brief This function handles USART1 global interrupt.
-  /* USER CODE END FDCAN1_IT1_IRQn 1 */
+  */
-}
+void USART1_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN USART1_IRQn 0 */
-  * @brief This function handles FDCAN2 interrupt 1.
+
-  */
+  /* USER CODE END USART1_IRQn 0 */
-void FDCAN2_IT1_IRQHandler(void)
+  HAL_UART_IRQHandler(&huart1);
-{
+  /* USER CODE BEGIN USART1_IRQn 1 */
-  /* USER CODE BEGIN FDCAN2_IT1_IRQn 0 */
+
-
+  /* USER CODE END USART1_IRQn 1 */
-  /* USER CODE END FDCAN2_IT1_IRQn 0 */
+}
-  HAL_FDCAN_IRQHandler(&hfdcan2);
+
-  /* USER CODE BEGIN FDCAN2_IT1_IRQn 1 */
+/**
-
+  * @brief This function handles USART2 global interrupt.
-  /* USER CODE END FDCAN2_IT1_IRQn 1 */
+  */
-}
+void USART2_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN USART2_IRQn 0 */
-  * @brief This function handles SPI2 global interrupt.
+
-  */
+  /* USER CODE END USART2_IRQn 0 */
-void SPI2_IRQHandler(void)
+  HAL_UART_IRQHandler(&huart2);
-{
+  /* USER CODE BEGIN USART2_IRQn 1 */
-  /* USER CODE BEGIN SPI2_IRQn 0 */
+
-
+  /* USER CODE END USART2_IRQn 1 */
-  /* USER CODE END SPI2_IRQn 0 */
+}
-  HAL_SPI_IRQHandler(&hspi2);
+
-  /* USER CODE BEGIN SPI2_IRQn 1 */
+/**
-
+  * @brief This function handles USART3 global interrupt.
-  /* USER CODE END SPI2_IRQn 1 */
+  */
-}
+void USART3_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN USART3_IRQn 0 */
-  * @brief This function handles USART1 global interrupt.
+
-  */
+  /* USER CODE END USART3_IRQn 0 */
-void USART1_IRQHandler(void)
+  HAL_UART_IRQHandler(&huart3);
-{
+  /* USER CODE BEGIN USART3_IRQn 1 */
-  /* USER CODE BEGIN USART1_IRQn 0 */
+
-
+  /* USER CODE END USART3_IRQn 1 */
-  /* USER CODE END USART1_IRQn 0 */
+}
-  HAL_UART_IRQHandler(&huart1);
+
-  /* USER CODE BEGIN USART1_IRQn 1 */
+/**
-  BSP_UART_IRQHandler(&huart1);
+  * @brief This function handles EXTI line[15:10] interrupts.
-
+  */
-  /* USER CODE END USART1_IRQn 1 */
+void EXTI15_10_IRQHandler(void)
-}
+{
-
+  /* USER CODE BEGIN EXTI15_10_IRQn 0 */
-/**
+
-  * @brief This function handles USART2 global interrupt.
+  /* USER CODE END EXTI15_10_IRQn 0 */
-  */
+  HAL_GPIO_EXTI_IRQHandler(ACCL_INT_Pin);
-void USART2_IRQHandler(void)
+  HAL_GPIO_EXTI_IRQHandler(GYRO_INT_Pin);
-{
+  /* USER CODE BEGIN EXTI15_10_IRQn 1 */
-  /* USER CODE BEGIN USART2_IRQn 0 */
+
-
+  /* USER CODE END EXTI15_10_IRQn 1 */
-  /* USER CODE END USART2_IRQn 0 */
+}
-  HAL_UART_IRQHandler(&huart2);
+
-  /* USER CODE BEGIN USART2_IRQn 1 */
+/**
-
+  * @brief This function handles UART5 global interrupt.
-  /* USER CODE END USART2_IRQn 1 */
+  */
-}
+void UART5_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN UART5_IRQn 0 */
-  * @brief This function handles USART3 global interrupt.
+
-  */
+  /* USER CODE END UART5_IRQn 0 */
-void USART3_IRQHandler(void)
+  HAL_UART_IRQHandler(&huart5);
-{
+  /* USER CODE BEGIN UART5_IRQn 1 */
-  /* USER CODE BEGIN USART3_IRQn 0 */
+
-
+  /* USER CODE END UART5_IRQn 1 */
-  /* USER CODE END USART3_IRQn 0 */
+}
-  HAL_UART_IRQHandler(&huart3);
+
-  /* USER CODE BEGIN USART3_IRQn 1 */
+/**
-
+  * @brief This function handles UART7 global interrupt.
-  /* USER CODE END USART3_IRQn 1 */
+  */
-}
+void UART7_IRQHandler(void)
-
+{
-/**
+  /* USER CODE BEGIN UART7_IRQn 0 */
-  * @brief This function handles EXTI line[15:10] interrupts.
+
-  */
+  /* USER CODE END UART7_IRQn 0 */
-void EXTI15_10_IRQHandler(void)
+  HAL_UART_IRQHandler(&huart7);
-{
+  /* USER CODE BEGIN UART7_IRQn 1 */
-  /* USER CODE BEGIN EXTI15_10_IRQn 0 */
+
-
+  /* USER CODE END UART7_IRQn 1 */
-  /* USER CODE END EXTI15_10_IRQn 0 */
+}
-  HAL_GPIO_EXTI_IRQHandler(ACCL_INT_Pin);
+
-  HAL_GPIO_EXTI_IRQHandler(GYRO_INT_Pin);
+/**
-  /* USER CODE BEGIN EXTI15_10_IRQn 1 */
+  * @brief This function handles USART10 global interrupt.
-
+  */
-  /* USER CODE END EXTI15_10_IRQn 1 */
+void USART10_IRQHandler(void)
-}
+{
-
+  /* USER CODE BEGIN USART10_IRQn 0 */
-/**
+
-  * @brief This function handles UART5 global interrupt.
+  /* USER CODE END USART10_IRQn 0 */
-  */
+  HAL_UART_IRQHandler(&huart10);
-void UART5_IRQHandler(void)
+  /* USER CODE BEGIN USART10_IRQn 1 */
-{
+
-  /* USER CODE BEGIN UART5_IRQn 0 */
+  /* USER CODE END USART10_IRQn 1 */
-
+}
-  /* USER CODE END UART5_IRQn 0 */
+
-  HAL_UART_IRQHandler(&huart5);
+/**
-  /* USER CODE BEGIN UART5_IRQn 1 */
+  * @brief This function handles FDCAN3 interrupt 0.
-  BSP_UART_IRQHandler(&huart5);
+  */
-
+void FDCAN3_IT0_IRQHandler(void)
-  /* USER CODE END UART5_IRQn 1 */
+{
-}
+  /* USER CODE BEGIN FDCAN3_IT0_IRQn 0 */
-
+
-/**
+  /* USER CODE END FDCAN3_IT0_IRQn 0 */
-  * @brief This function handles UART7 global interrupt.
+  HAL_FDCAN_IRQHandler(&hfdcan3);
-  */
+  /* USER CODE BEGIN FDCAN3_IT0_IRQn 1 */
-void UART7_IRQHandler(void)
+
-{
+  /* USER CODE END FDCAN3_IT0_IRQn 1 */
-  /* USER CODE BEGIN UART7_IRQn 0 */
+}
-
+
-  /* USER CODE END UART7_IRQn 0 */
+/**
-  HAL_UART_IRQHandler(&huart7);
+  * @brief This function handles FDCAN3 interrupt 1.
-  /* USER CODE BEGIN UART7_IRQn 1 */
+  */
-
+void FDCAN3_IT1_IRQHandler(void)
-  /* USER CODE END UART7_IRQn 1 */
+{
-}
+  /* USER CODE BEGIN FDCAN3_IT1_IRQn 0 */
-
+
-/**
+  /* USER CODE END FDCAN3_IT1_IRQn 0 */
-  * @brief This function handles ADC3 global interrupt.
+  HAL_FDCAN_IRQHandler(&hfdcan3);
-  */
+  /* USER CODE BEGIN FDCAN3_IT1_IRQn 1 */
-void ADC3_IRQHandler(void)
+
-{
+  /* USER CODE END FDCAN3_IT1_IRQn 1 */
-  /* USER CODE BEGIN ADC3_IRQn 0 */
+}
-
+
-  /* USER CODE END ADC3_IRQn 0 */
+/**
-  HAL_ADC_IRQHandler(&hadc3);
+  * @brief This function handles TIM23 global interrupt.
-  /* USER CODE BEGIN ADC3_IRQn 1 */
+  */
-
+void TIM23_IRQHandler(void)
-  /* USER CODE END ADC3_IRQn 1 */
+{
-}
+  /* USER CODE BEGIN TIM23_IRQn 0 */
-
+
-/**
+  /* USER CODE END TIM23_IRQn 0 */
-  * @brief This function handles BDMA channel0 global interrupt.
+  HAL_TIM_IRQHandler(&htim23);
-  */
+  /* USER CODE BEGIN TIM23_IRQn 1 */
-void BDMA_Channel0_IRQHandler(void)
+
-{
+  /* USER CODE END TIM23_IRQn 1 */
-  /* USER CODE BEGIN BDMA_Channel0_IRQn 0 */
+}
-
+
-  /* USER CODE END BDMA_Channel0_IRQn 0 */
+/* USER CODE BEGIN 1 */
-  HAL_DMA_IRQHandler(&hdma_adc3);
+
-  /* USER CODE BEGIN BDMA_Channel0_IRQn 1 */
+/* USER CODE END 1 */
-
-  /* USER CODE END BDMA_Channel0_IRQn 1 */
-}
-
-/**
-  * @brief This function handles USART10 global interrupt.
-  */
-void USART10_IRQHandler(void)
-{
-  /* USER CODE BEGIN USART10_IRQn 0 */
-
-  /* USER CODE END USART10_IRQn 0 */
-  HAL_UART_IRQHandler(&huart10);
-  /* USER CODE BEGIN USART10_IRQn 1 */
-
-  /* USER CODE END USART10_IRQn 1 */
-}
-
-/**
-  * @brief This function handles FDCAN3 interrupt 0.
-  */
-void FDCAN3_IT0_IRQHandler(void)
-{
-  /* USER CODE BEGIN FDCAN3_IT0_IRQn 0 */
-
-  /* USER CODE END FDCAN3_IT0_IRQn 0 */
-  HAL_FDCAN_IRQHandler(&hfdcan3);
-  /* USER CODE BEGIN FDCAN3_IT0_IRQn 1 */
-
-  /* USER CODE END FDCAN3_IT0_IRQn 1 */
-}
-
-/**
-  * @brief This function handles FDCAN3 interrupt 1.
-  */
-void FDCAN3_IT1_IRQHandler(void)
-{
-  /* USER CODE BEGIN FDCAN3_IT1_IRQn 0 */
-
-  /* USER CODE END FDCAN3_IT1_IRQn 0 */
-  HAL_FDCAN_IRQHandler(&hfdcan3);
-  /* USER CODE BEGIN FDCAN3_IT1_IRQn 1 */
-
-  /* USER CODE END FDCAN3_IT1_IRQn 1 */
-}
-
-/**
-  * @brief This function handles TIM23 global interrupt.
-  */
-void TIM23_IRQHandler(void)
-{
-  /* USER CODE BEGIN TIM23_IRQn 0 */
-
-  /* USER CODE END TIM23_IRQn 0 */
-  HAL_TIM_IRQHandler(&htim23);
-  /* USER CODE BEGIN TIM23_IRQn 1 */
-
-  /* USER CODE END TIM23_IRQn 1 */
-}
-
-/* USER CODE BEGIN 1 */
-
-/* USER CODE END 1 */

Core/Src/usart.c

@@ -31,8 +31,6 @@ 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)
@@ -433,43 +431,6 @@ 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);
@@ -669,10 +630,6 @@ 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,33 +16,6 @@ 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=
@@ -78,9 +51,7 @@ Dma.Request0=ADC1
 Dma.Request1=SPI2_RX
 Dma.Request2=SPI2_TX
 Dma.Request3=UART5_RX
-Dma.Request4=USART1_TX
+Dma.RequestsNb=4
-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
@@ -135,42 +106,6 @@ 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
@@ -206,11 +141,9 @@ FDCAN3.RxFifo0ElmtsNbr=32
 FDCAN3.RxFifo1ElmtsNbr=32
 FDCAN3.StdFiltersNbr=1
 FDCAN3.TxFifoQueueElmtsNbr=32
-FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE,configUSE_STATS_FORMATTING_FUNCTIONS,configGENERATE_RUN_TIME_STATS
+FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE
 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
@@ -219,34 +152,32 @@ MMTConfigApplied=false
 Mcu.CPN=STM32H723VGT6
 Mcu.Family=STM32H7
 Mcu.IP0=ADC1
-Mcu.IP1=ADC3
+Mcu.IP1=CORTEX_M7
-Mcu.IP10=MEMORYMAP
+Mcu.IP10=OCTOSPI1
-Mcu.IP11=NVIC
+Mcu.IP11=RCC
-Mcu.IP12=OCTOSPI1
+Mcu.IP12=SPI1
-Mcu.IP13=RCC
+Mcu.IP13=SPI2
-Mcu.IP14=SPI1
+Mcu.IP14=SYS
-Mcu.IP15=SPI2
+Mcu.IP15=TIM1
-Mcu.IP16=SYS
+Mcu.IP16=TIM2
-Mcu.IP17=TIM1
+Mcu.IP17=TIM3
-Mcu.IP18=TIM2
+Mcu.IP18=TIM12
-Mcu.IP19=TIM3
+Mcu.IP19=UART5
-Mcu.IP2=BDMA
+Mcu.IP2=DEBUG
-Mcu.IP20=TIM12
+Mcu.IP20=UART7
-Mcu.IP21=UART5
+Mcu.IP21=USART1
-Mcu.IP22=UART7
+Mcu.IP22=USART2
-Mcu.IP23=USART1
+Mcu.IP23=USART3
-Mcu.IP24=USART2
+Mcu.IP24=USART10
-Mcu.IP25=USART3
+Mcu.IP25=USB_OTG_HS
-Mcu.IP26=USART10
+Mcu.IP3=DMA
-Mcu.IP27=USB_OTG_HS
+Mcu.IP4=FDCAN1
-Mcu.IP3=CORTEX_M7
+Mcu.IP5=FDCAN2
-Mcu.IP4=DEBUG
+Mcu.IP6=FDCAN3
-Mcu.IP5=DMA
+Mcu.IP7=FREERTOS
-Mcu.IP6=FDCAN1
+Mcu.IP8=MEMORYMAP
-Mcu.IP7=FDCAN2
+Mcu.IP9=NVIC
-Mcu.IP8=FDCAN3
+Mcu.IPNb=26
-Mcu.IP9=FREERTOS
-Mcu.IPNb=28
 Mcu.Name=STM32H723VGTx
 Mcu.Package=LQFP100
 Mcu.Pin0=PE2
@@ -307,33 +238,24 @@ Mcu.Pin58=PB3(JTDO/TRACESWO)
 Mcu.Pin59=PB5
 Mcu.Pin6=PH1-OSC_OUT
 Mcu.Pin60=PB6
-Mcu.Pin61=VP_ADC3_TempSens_Input
+Mcu.Pin61=VP_FREERTOS_VS_CMSIS_V2
-Mcu.Pin62=VP_ADC3_Vref_Input
+Mcu.Pin62=VP_OCTOSPI1_VS_octo
-Mcu.Pin63=VP_ADC3_Vbat_Input
+Mcu.Pin63=VP_SYS_VS_tim23
-Mcu.Pin64=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin64=VP_MEMORYMAP_VS_MEMORYMAP
-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=69
+Mcu.PinsNb=65
-Mcu.ThirdParty0=STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0
+Mcu.ThirdPartyNb=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
@@ -587,12 +509,12 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
 ProjectManager.ProjectStructure=
 ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x2000
-ProjectManager.TargetToolchain=CMake
+ProjectManager.TargetToolchain=MDK-ARM V5.32
 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,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
+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
 RCC.ADCFreq_Value=96000000
 RCC.AHB12Freq_Value=240000000
 RCC.AHB4Freq_Value=240000000
@@ -719,10 +641,6 @@ 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
@@ -770,20 +688,12 @@ 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,18 +1323,6 @@
       <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>
@@ -1345,7 +1333,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1357,7 +1345,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1369,7 +1357,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1381,7 +1369,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1401,7 +1389,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1413,7 +1401,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1425,7 +1413,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1437,7 +1425,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1449,7 +1437,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1461,7 +1449,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1473,7 +1461,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1485,7 +1473,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>97</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1497,7 +1485,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>98</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1509,7 +1497,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>99</FileNumber>
+      <FileNumber>98</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1521,7 +1509,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>100</FileNumber>
+      <FileNumber>99</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1531,18 +1519,6 @@
       <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,11 +839,6 @@
               <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>
@@ -929,11 +924,6 @@
               <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

@@ -1,201 +0,0 @@
-                                 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,6 +124,4 @@ uart:
   devices:
   - instance: UART5
     name: DR16
-  - instance: USART1
-    name: VOFA
   enabled: true

User/bsp/uart.c

@@ -25,8 +25,6 @@ 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;
 }
@@ -117,8 +115,6 @@ 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,7 +28,6 @@ extern "C" {
 /* UART实体枚举，与设备对应 */
 typedef enum {
   BSP_UART_DR16,
-  BSP_UART_VOFA,
   BSP_UART_NUM,
   BSP_UART_ERR,
 } BSP_UART_t;

User/component/QuaternionEKF.c

@@ -1,527 +0,0 @@
-/*
-  四元数扩展卡尔曼滤波器（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

@@ -1,111 +0,0 @@
-/*
-  四元数扩展卡尔曼滤波器（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,9 +28,6 @@
 #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

@@ -1,591 +0,0 @@
-/*
-  卡尔曼滤波器 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

@@ -1,199 +0,0 @@
-/*
-  卡尔曼滤波器
-  支持动态量测调整，使用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,9 +61,6 @@ 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;
@@ -127,9 +124,8 @@ 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);
     
-    // 计算角加速度并滤波
+    // 计算角加速度
-    float dd_theta_raw = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
+    leg->dd_theta = 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,7 +8,6 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 #include <math.h>
-#include "filter.h"
 /* Exported types ----------------------------------------------------------- */
 
 /**
@@ -84,7 +83,6 @@ 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,6 +1,5 @@
 #pragma once
 
-#include <sys/_intsup.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -31,7 +30,6 @@ typedef struct {
   } gyro_offset; /* 陀螺仪偏置 */
 } BMI088_Cali_t; /* BMI088校准数据 */
 
-
 typedef struct {
   DEVICE_Header_t header;
   AHRS_Accl_t accl;

User/device/device.h

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

User/device/device_config.yaml

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

User/device/mrobot.c

@@ -1,873 +0,0 @@
-/**
- * @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

@@ -1,325 +0,0 @@
-/**
- * @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

@@ -1,106 +0,0 @@
-/* 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

@@ -1,39 +0,0 @@
-#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=2000.0f;  /* 归一化目标转速 */
+    s->target_variable.target_rpm=6000.0f;  /* 归一化目标转速 */
 
 	return 0;
 }

User/task/ai.c

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

User/task/atti_esit.c

@@ -10,14 +10,10 @@
 #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 ---------------------------------------------------------- */
@@ -26,10 +22,9 @@
 /* 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;
@@ -51,27 +46,6 @@ 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，消除警告 */
@@ -81,17 +55,10 @@ 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) {
@@ -111,19 +78,10 @@ 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

@@ -1,40 +0,0 @@
-/*
-    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,16 +53,3 @@
   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

@@ -68,9 +68,7 @@ void Task_ctrl_chassis(void *argument) {
       chassis_cmd.jump_trigger = false;  /* 清除触发标志 */
     }
 
-    Chassis_Control(&chassis, &chassis_cmd);  
+    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

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

User/task/init.c

@@ -40,8 +40,6 @@ 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 */
@@ -50,10 +48,8 @@ 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

@@ -48,14 +48,4 @@ const osThreadAttr_t attr_ai = {
     .name = "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,7 +20,6 @@ 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)
@@ -32,8 +31,6 @@ 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 ----------------------------------------------------------- */
@@ -51,8 +48,6 @@ typedef struct {
         osThreadId_t blink;
         osThreadId_t ctrl_shoot;
         osThreadId_t ai;
-        osThreadId_t vofa;
-        osThreadId_t cli;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -62,7 +57,6 @@ typedef struct {
             osMessageQueueId_t imu;
             osMessageQueueId_t cmd;
             osMessageQueueId_t yaw;
-            osMessageQueueId_t vofa;
         }chassis;
         struct {
             osMessageQueueId_t imu;
@@ -107,8 +101,6 @@ typedef struct {
         UBaseType_t blink;
         UBaseType_t ctrl_shoot;
         UBaseType_t ai;
-        UBaseType_t vofa;
-        UBaseType_t cli;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -120,7 +112,6 @@ typedef struct {
         float blink;
         float ctrl_shoot;
         float ai;
-        float vofa;
     } freq;
 
     /* 任务最近运行时间 */
@@ -132,7 +123,6 @@ typedef struct {
         float blink;
         float ctrl_shoot;
         float ai;
-        float vofa;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -150,8 +140,6 @@ 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);
@@ -163,8 +151,6 @@ 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

@@ -1,84 +0,0 @@
-/*
-    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,7 +19,6 @@ 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
@@ -28,7 +27,6 @@ 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