From de8a0d51179dcfae1d9dfbc87bbed33a3ed901c4 Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Sat, 15 Nov 2025 22:18:00 +0800
Subject: [PATCH] imu test failed

---
 Core/Inc/main.h                               |   8 +-
 Core/Src/gpio.c                               |   6 +-
 Core/Src/main.c                               | 269 +++++-----
 TronOneH7_Scaffold.ioc                        |   4 +-
 .../algorithm/QuaternionEKF/QuaternionEKF.c   | 508 ++++++++++++++++++
 .../algorithm/QuaternionEKF/QuaternionEKF.h   |  78 +++
 .../module/algorithm/controller/controller.c  |   5 +
 .../module/algorithm/controller/controller.h  |   8 +
 User_Code/module/algorithm/crc/crc16.c        |   5 +
 User_Code/module/algorithm/crc/crc16.h        |   8 +
 User_Code/module/algorithm/crc/crc8.c         |   5 +
 User_Code/module/algorithm/crc/crc8.h         |   8 +
 .../algorithm/kalman_filter/kalman_filter.c   |   5 +
 .../algorithm/kalman_filter/kalman_filter.h   |   8 +
 .../module/algorithm/user_lib/user_lib.c      | 215 ++++++++
 .../module/algorithm/user_lib/user_lib.h      | 165 ++++++
 .../module/periph/imu/BMI088Middleware.c      |  31 ++
 .../module/periph/imu/BMI088Middleware.h      |  34 ++
 User_Code/module/periph/imu/BMI088driver.c    | 416 ++++++++++++++
 User_Code/module/periph/imu/BMI088driver.h    | 133 +++++
 User_Code/module/periph/imu/BMI088reg.h       | 180 +++++++
 User_Code/module/periph/imu/ins_task.c        | 351 ++++++++++++
 User_Code/module/periph/imu/ins_task.h        | 143 +++++
 ozonedeb/windebnewestux.jdebug.user           |  22 +-
 24 files changed, 2458 insertions(+), 157 deletions(-)
 create mode 100644 User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.c
 create mode 100644 User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.h
 create mode 100644 User_Code/module/algorithm/controller/controller.c
 create mode 100644 User_Code/module/algorithm/controller/controller.h
 create mode 100644 User_Code/module/algorithm/crc/crc16.c
 create mode 100644 User_Code/module/algorithm/crc/crc16.h
 create mode 100644 User_Code/module/algorithm/crc/crc8.c
 create mode 100644 User_Code/module/algorithm/crc/crc8.h
 create mode 100644 User_Code/module/algorithm/kalman_filter/kalman_filter.c
 create mode 100644 User_Code/module/algorithm/kalman_filter/kalman_filter.h
 create mode 100644 User_Code/module/algorithm/user_lib/user_lib.c
 create mode 100644 User_Code/module/algorithm/user_lib/user_lib.h
 create mode 100644 User_Code/module/periph/imu/BMI088Middleware.c
 create mode 100644 User_Code/module/periph/imu/BMI088Middleware.h
 create mode 100644 User_Code/module/periph/imu/BMI088driver.c
 create mode 100644 User_Code/module/periph/imu/BMI088driver.h
 create mode 100644 User_Code/module/periph/imu/BMI088reg.h
 create mode 100644 User_Code/module/periph/imu/ins_task.c
 create mode 100644 User_Code/module/periph/imu/ins_task.h

diff --git a/Core/Inc/main.h b/Core/Inc/main.h
index 3ec5932..d3dfa02 100644
--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -61,10 +61,10 @@ void Error_Handler(void);
 #define ACC_CS_GPIO_Port GPIOC
 #define GYRO_CS_Pin GPIO_PIN_3
 #define GYRO_CS_GPIO_Port GPIOC
-#define pump1_Pin GPIO_PIN_0
-#define pump1_GPIO_Port GPIOA
-#define pump2_Pin GPIO_PIN_2
-#define pump2_GPIO_Port GPIOA
+#define power2_Pin GPIO_PIN_0
+#define power2_GPIO_Port GPIOA
+#define power1_Pin GPIO_PIN_2
+#define power1_GPIO_Port GPIOA
 #define pump3_Pin GPIO_PIN_9
 #define pump3_GPIO_Port GPIOE
 #define ACC_INT_Pin GPIO_PIN_10
diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c
index 1bdee4c..bdbf0a7 100644
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -62,7 +62,7 @@ void MX_GPIO_Init(void)
   HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15|GYRO_CS_Pin, GPIO_PIN_SET);
 
   /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOA, pump1_Pin|pump2_Pin, GPIO_PIN_SET);
+  HAL_GPIO_WritePin(GPIOA, power2_Pin|power1_Pin, GPIO_PIN_SET);
 
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GPIOE, pump3_Pin|pump4_Pin, GPIO_PIN_RESET);
@@ -90,8 +90,8 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
   HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
-  /*Configure GPIO pins : pump1_Pin pump2_Pin */
-  GPIO_InitStruct.Pin = pump1_Pin|pump2_Pin;
+  /*Configure GPIO pins : power2_Pin power1_Pin */
+  GPIO_InitStruct.Pin = power2_Pin|power1_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_PULLDOWN;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 52478bc..95aefb1 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -60,11 +60,8 @@
 
 /* Private function prototypes -----------------------------------------------*/
 void SystemClock_Config(void);
-
 static void MPU_Config(void);
-
 void MX_FREERTOS_Init(void);
-
 /* USER CODE BEGIN PFP */
 
 /* USER CODE END PFP */
@@ -81,83 +78,83 @@ void MX_FREERTOS_Init(void);
 int main(void)
 {
 
-    /* USER CODE BEGIN 1 */
+  /* USER CODE BEGIN 1 */
 
-    /* USER CODE END 1 */
+  /* USER CODE END 1 */
 
-    /* MPU Configuration--------------------------------------------------------*/
-    MPU_Config();
+  /* MPU Configuration--------------------------------------------------------*/
+  MPU_Config();
 
-    /* Enable the CPU Cache */
+  /* Enable the CPU Cache */
 
-    /* Enable I-Cache---------------------------------------------------------*/
-    SCB_EnableICache();
+  /* Enable I-Cache---------------------------------------------------------*/
+  SCB_EnableICache();
 
-    /* Enable D-Cache---------------------------------------------------------*/
-    SCB_EnableDCache();
+  /* Enable D-Cache---------------------------------------------------------*/
+  SCB_EnableDCache();
 
-    /* MCU Configuration--------------------------------------------------------*/
+  /* MCU Configuration--------------------------------------------------------*/
 
-    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
-    HAL_Init();
+  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+  HAL_Init();
 
-    /* USER CODE BEGIN Init */
+  /* USER CODE BEGIN Init */
 
-    /* USER CODE END Init */
+  /* USER CODE END Init */
 
-    /* Configure the system clock */
-    SystemClock_Config();
+  /* Configure the system clock */
+  SystemClock_Config();
 
-    /* USER CODE BEGIN SysInit */
+  /* USER CODE BEGIN SysInit */
 
-    /* USER CODE END SysInit */
+  /* USER CODE END SysInit */
 
-    /* Initialize all configured peripherals */
-    MX_GPIO_Init();
-    MX_DMA_Init();
-    MX_FDCAN2_Init();
-    MX_FDCAN3_Init();
-    MX_CRC_Init();
-    MX_FDCAN1_Init();
-    MX_USART10_UART_Init();
-    MX_SPI2_Init();
-    MX_TIM12_Init();
-    MX_UART7_Init();
-    MX_TIM1_Init();
-    MX_USART1_UART_Init();
-    MX_ADC3_Init();
-    MX_USART2_UART_Init();
-    MX_USART3_UART_Init();
-    MX_SPI1_Init();
-    MX_UART5_Init();
-    MX_TIM3_Init();
-    MX_SPI6_Init();
-    /* USER CODE BEGIN 2 */
+  /* Initialize all configured peripherals */
+  MX_GPIO_Init();
+  MX_DMA_Init();
+  MX_FDCAN2_Init();
+  MX_FDCAN3_Init();
+  MX_CRC_Init();
+  MX_FDCAN1_Init();
+  MX_USART10_UART_Init();
+  MX_SPI2_Init();
+  MX_TIM12_Init();
+  MX_UART7_Init();
+  MX_TIM1_Init();
+  MX_USART1_UART_Init();
+  MX_ADC3_Init();
+  MX_USART2_UART_Init();
+  MX_USART3_UART_Init();
+  MX_SPI1_Init();
+  MX_UART5_Init();
+  MX_TIM3_Init();
+  MX_SPI6_Init();
+  /* USER CODE BEGIN 2 */
 
     robotSelfCheck(); //自检LED
     systemstart_song(); //开机音乐
 
     /*还没有启动调度器！不允许任何涉及到 Freertos ticks的内容在这里运行！！！*/
-    /* USER CODE END 2 */
+  /* USER CODE END 2 */
 
-    /* Init scheduler */
-    osKernelInitialize(); /* Call init function for freertos objects (in cmsis_os2.c) */
-    MX_FREERTOS_Init();
+  /* Init scheduler */
+  osKernelInitialize();  /* Call init function for freertos objects (in cmsis_os2.c) */
+  MX_FREERTOS_Init();
 
-    /* Start scheduler */
-    osKernelStart();
+  /* Start scheduler */
+  osKernelStart();
 
-    /* We should never get here as control is now taken by the scheduler */
+  /* We should never get here as control is now taken by the scheduler */
 
-    /* Infinite loop */
-    /* USER CODE BEGIN WHILE */
+  /* Infinite loop */
+  /* USER CODE BEGIN WHILE */
     while (1)
     {
-        /* USER CODE END WHILE */
+    /* USER CODE END WHILE */
 
-        /* USER CODE BEGIN 3 */
+    /* USER CODE BEGIN 3 */
     }
-    /* USER CODE END 3 */
+  /* USER CODE END 3 */
 }
 
 /**
@@ -166,95 +163,93 @@ int main(void)
   */
 void SystemClock_Config(void)
 {
-    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
-    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
 
-    /** Supply configuration update enable
-    */
-    HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);
+  /** Supply configuration update enable
+  */
+  HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);
 
-    /** Configure the main internal regulator output voltage
-    */
-    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
+  /** Configure the main internal regulator output voltage
+  */
+  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
 
-    while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY))
-    {
-    }
+  while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}
 
-    /** Initializes the RCC Oscillators according to the specified parameters
-    * in the RCC_OscInitTypeDef structure.
-    */
-    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSI
-                                       | RCC_OSCILLATORTYPE_HSE;
-    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
-    RCC_OscInitStruct.HSIState = RCC_HSI_DIV1;
-    RCC_OscInitStruct.HSICalibrationValue = 64;
-    RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
-    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
-    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
-    RCC_OscInitStruct.PLL.PLLM = 2;
-    RCC_OscInitStruct.PLL.PLLN = 40;
-    RCC_OscInitStruct.PLL.PLLP = 1;
-    RCC_OscInitStruct.PLL.PLLQ = 4;
-    RCC_OscInitStruct.PLL.PLLR = 2;
-    RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
-    RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
-    RCC_OscInitStruct.PLL.PLLFRACN = 0;
-    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
-    {
-        Error_Handler();
-    }
+  /** Initializes the RCC Oscillators according to the specified parameters
+  * in the RCC_OscInitTypeDef structure.
+  */
+  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48|RCC_OSCILLATORTYPE_HSI
+                              |RCC_OSCILLATORTYPE_HSE;
+  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+  RCC_OscInitStruct.HSIState = RCC_HSI_DIV1;
+  RCC_OscInitStruct.HSICalibrationValue = 64;
+  RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
+  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+  RCC_OscInitStruct.PLL.PLLM = 2;
+  RCC_OscInitStruct.PLL.PLLN = 40;
+  RCC_OscInitStruct.PLL.PLLP = 1;
+  RCC_OscInitStruct.PLL.PLLQ = 4;
+  RCC_OscInitStruct.PLL.PLLR = 2;
+  RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
+  RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
+  RCC_OscInitStruct.PLL.PLLFRACN = 0;
+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+  {
+    Error_Handler();
+  }
 
-    /** Initializes the CPU, AHB and APB buses clocks
-    */
-    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
-                                  | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2
-                                  | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1;
-    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
-    RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
-    RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
-    RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
-    RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
-    RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
-    RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
+  /** Initializes the CPU, AHB and APB buses clocks
+  */
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
+                              |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
+  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
+  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
+  RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
+  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
+  RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
+  RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
 
-    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
-    {
-        Error_Handler();
-    }
-    HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1);
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1);
 }
 
 /* USER CODE BEGIN 4 */
 
 /* USER CODE END 4 */
 
-/* MPU Configuration */
+ /* MPU Configuration */
 
 void MPU_Config(void)
 {
-    MPU_Region_InitTypeDef MPU_InitStruct = {0};
+  MPU_Region_InitTypeDef MPU_InitStruct = {0};
 
-    /* Disables the MPU */
-    HAL_MPU_Disable();
+  /* Disables the MPU */
+  HAL_MPU_Disable();
 
-    /** Initializes and configures the Region and the memory to be protected
-    */
-    MPU_InitStruct.Enable = MPU_REGION_ENABLE;
-    MPU_InitStruct.Number = MPU_REGION_NUMBER0;
-    MPU_InitStruct.BaseAddress = 0x24000000;
-    MPU_InitStruct.Size = MPU_REGION_SIZE_128KB;
-    MPU_InitStruct.SubRegionDisable = 0x0;
-    MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL1;
-    MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
-    MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
-    MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
-    MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
-    MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
+  /** Initializes and configures the Region and the memory to be protected
+  */
+  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
+  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
+  MPU_InitStruct.BaseAddress = 0x24000000;
+  MPU_InitStruct.Size = MPU_REGION_SIZE_128KB;
+  MPU_InitStruct.SubRegionDisable = 0x0;
+  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL1;
+  MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
+  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
+  MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
+  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
+  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
 
-    HAL_MPU_ConfigRegion(&MPU_InitStruct);
-    /* Enables the MPU */
-    HAL_MPU_Enable(MPU_HFNMI_PRIVDEF);
+  HAL_MPU_ConfigRegion(&MPU_InitStruct);
+  /* Enables the MPU */
+  HAL_MPU_Enable(MPU_HFNMI_PRIVDEF);
 
 }
 
@@ -268,16 +263,16 @@ void MPU_Config(void)
   */
 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
 {
-    /* USER CODE BEGIN Callback 0 */
+  /* USER CODE BEGIN Callback 0 */
 
-    /* USER CODE END Callback 0 */
-    if (htim->Instance == TIM23)
-    {
-        HAL_IncTick();
-    }
-    /* USER CODE BEGIN Callback 1 */
+  /* USER CODE END Callback 0 */
+  if (htim->Instance == TIM23)
+  {
+    HAL_IncTick();
+  }
+  /* USER CODE BEGIN Callback 1 */
 
-    /* USER CODE END Callback 1 */
+  /* USER CODE END Callback 1 */
 }
 
 /**
@@ -286,13 +281,13 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
   */
 void Error_Handler(void)
 {
-    /* USER CODE BEGIN Error_Handler_Debug */
+  /* USER CODE BEGIN Error_Handler_Debug */
     /* User can add his own implementation to report the HAL error return state */
     __disable_irq();
     while (1)
     {
     }
-    /* USER CODE END Error_Handler_Debug */
+  /* USER CODE END Error_Handler_Debug */
 }
 #ifdef USE_FULL_ASSERT
 /**
@@ -304,9 +299,9 @@ void Error_Handler(void)
   */
 void assert_failed(uint8_t *file, uint32_t line)
 {
-    /* USER CODE BEGIN 6 */
+  /* USER CODE BEGIN 6 */
     /* User can add his own implementation to report the file name and line number,
        ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
-    /* USER CODE END 6 */
+  /* USER CODE END 6 */
 }
 #endif /* USE_FULL_ASSERT */
diff --git a/TronOneH7_Scaffold.ioc b/TronOneH7_Scaffold.ioc
index 2c33e56..44f7de5 100644
--- a/TronOneH7_Scaffold.ioc
+++ b/TronOneH7_Scaffold.ioc
@@ -552,7 +552,7 @@ NVIC.USART2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.USART3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 PA0.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
-PA0.GPIO_Label=pump1
+PA0.GPIO_Label=power2
 PA0.GPIO_PuPd=GPIO_PULLDOWN
 PA0.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
 PA0.Locked=true
@@ -567,7 +567,7 @@ PA12.Locked=true
 PA12.Mode=Device_Only_FS
 PA12.Signal=USB_OTG_HS_DP
 PA2.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
-PA2.GPIO_Label=pump2
+PA2.GPIO_Label=power1
 PA2.GPIO_PuPd=GPIO_PULLDOWN
 PA2.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
 PA2.Locked=true
diff --git a/User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.c b/User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.c
new file mode 100644
index 0000000..27e857a
--- /dev/null
+++ b/User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.c
@@ -0,0 +1,508 @@
+/**
+ ******************************************************************************
+ * @file    QuaternionEKF.c
+ * @author  Wang Hongxi
+ * @version V1.2.0
+ * @date    2022/3/8
+ * @brief   attitude update with gyro bias estimate and chi-square test
+ ******************************************************************************
+ * @attention
+ * 1st order LPF transfer function:
+ *     1
+ *  ———————
+ *  as + 1
+ *
+ ******************************************************************************
+ */
+#include "QuaternionEKF.h"
+
+QEKF_INS_t QEKF_INS;
+
+const float IMU_QuaternionEKF_F[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};
+float IMU_QuaternionEKF_P[36] = {100000, 0.1, 0.1, 0.1, 0.1, 0.1,
+                                 0.1, 100000, 0.1, 0.1, 0.1, 0.1,
+                                 0.1, 0.1, 100000, 0.1, 0.1, 0.1,
+                                 0.1, 0.1, 0.1, 100000, 0.1, 0.1,
+                                 0.1, 0.1, 0.1, 0.1, 100, 0.1,
+                                 0.1, 0.1, 0.1, 0.1, 0.1, 100};
+float IMU_QuaternionEKF_K[18];
+float IMU_QuaternionEKF_H[18];
+
+static float invSqrt(float x);
+
+static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf);
+
+static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
+
+static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
+
+static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
+
+/**
+ * @brief Quaternion EKF initialization and some reference value
+ * @param[in] process_noise1 quaternion process noise    10
+ * @param[in] process_noise2 gyro bias process noise     0.001
+ * @param[in] measure_noise  accel measure noise         1000000
+ * @param[in] lambda         fading coefficient          0.9996
+ * @param[in] lpf            lowpass filter coefficient  0
+ */
+void IMU_QuaternionEKF_Init(float *init_quaternion, 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.ChiSquareTestThreshold = 1e-8;
+    QEKF_INS.ConvergeFlag = 0;
+    QEKF_INS.ErrorCount = 0;
+    QEKF_INS.UpdateCount = 0;
+    if (lambda > 1)
+    {
+        lambda = 1;
+    }
+    QEKF_INS.lambda = lambda;
+    QEKF_INS.accLPFcoef = lpf;
+
+    // 初始化矩阵维度信息
+    Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3);
+    Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *) QEKF_INS.ChiSquare_Data);
+
+    // 姿态初始化
+    for (int i = 0; i < 4; i++)
+    {
+        QEKF_INS.IMU_QuaternionEKF.xhat_data[i] = init_quaternion[i];
+    }
+
+    // 自定义函数初始化,用于扩展或增加kf的基础功能
+    QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
+    QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading;
+    QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH;
+    QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate;
+
+    // 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合)
+    QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE;
+    QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE;
+
+    memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
+    memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P));
+}
+
+/**
+ * @brief Quaternion EKF update
+ * @param[in]       gyro x y z in rad/s
+ * @param[in]       accel x y z in m/s²
+ * @param[in]       update period in s
+ */
+void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt)
+{
+    // 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
+    static float halfgxdt, halfgydt, halfgzdt;
+    static float accelInvNorm;
+
+    /*   F, number with * represent vals to be set
+     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.GyroBias[0];
+    QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
+    QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];
+
+    // set 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用单位阵覆盖前一轮线性化后的矩阵
+    memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
+
+    QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt;
+
+    QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt;
+
+    QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt;
+
+    QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt;
+    QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
+
+    // accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
+    if (QEKF_INS.UpdateCount == 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.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt
+                        / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
+    QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt
+                        / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
+    QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt
+                        / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
+
+    // set z,单位化重力加速度向量
+    accelInvNorm = 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]);
+    // accelInvNorm = invSqrt(1);
+    for (uint8_t i = 0; i < 3; ++i)
+    {
+        QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
+    }
+
+    // get body state
+    QEKF_INS.gyro_norm = 1.0f / 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 / accelInvNorm;
+
+    // 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
+    // 稍后在最后的姿态更新部分会利用StableFlag来确定
+    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.StableFlag = 1;
+    }
+    else
+    {
+        QEKF_INS.StableFlag = 0;
+    }
+
+    // set Q R,过程噪声和观测噪声矩阵
+    QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
+    QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
+    QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
+    QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
+    QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
+    QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
+    QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R;
+    QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
+    QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;
+
+    // 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
+    Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);
+
+    // 获取融合后的数据,包括四元数和xy零飘值
+    QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
+    QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
+    QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
+    QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];
+    QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
+    QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
+    QEKF_INS.GyroBias[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 = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Roll = 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;
+    // 翻了的话改变一下2.0前面的符号，长短边朝前也记得调整一下=号右边的内容就ok
+
+    // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
+    if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
+    {
+        QEKF_INS.YawRoundCount--;
+    }
+    else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f)
+    {
+        QEKF_INS.YawRoundCount++;
+    }
+    QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
+    QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
+    QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
+}
+
+/**
+ * @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新;
+ *        并对零漂的方差进行限制,防止过度收敛并限幅防止发散
+ *
+ * @param kf
+ */
+static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
+{
+    static float q0, q1, q2, q3;
+    static float qInvNorm;
+
+    q0 = kf->xhatminus_data[0];
+    q1 = kf->xhatminus_data[1];
+    q2 = kf->xhatminus_data[2];
+    q3 = kf->xhatminus_data[3];
+
+    // quaternion normalize
+    qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    for (uint8_t i = 0; i < 4; ++i)
+    {
+        kf->xhatminus_data[i] *= qInvNorm;
+    }
+    /*  F, number with * represent vals to be set
+     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
+    */
+    // set F
+    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;
+
+    // fading filter,防止零飘参数过度收敛
+    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 IMU_QuaternionEKF_SetH(KalmanFilter_t *kf)
+{
+    static float doubleq0, doubleq1, doubleq2, doubleq3;
+    /* H
+     0     1     2     3     4     5
+     6     7     8     9    10    11
+    12    13    14    15    16    17
+    last two cols are zero
+    */
+    // set H
+    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->zSize * kf->xhatSize);
+
+    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 IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
+{
+    static float q0, q1, q2, q3;
+
+    kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
+    kf->temp_matrix.numRows = kf->H.numRows;
+    kf->temp_matrix.numCols = kf->Pminus.numCols;
+    kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
+    kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
+    kf->temp_matrix1.numCols = kf->HT.numCols;
+    kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
+    kf->S.numRows = kf->R.numRows;
+    kf->S.numCols = kf->R.numCols;
+    kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
+    kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
+
+    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);
+    kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
+
+    // 计算预测值和各个轴的方向余弦
+    for (uint8_t i = 0; i < 3; ++i)
+    {
+        QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
+    }
+
+    // 利用加速度计数据修正
+    kf->temp_vector1.numRows = kf->z.numRows;
+    kf->temp_vector1.numCols = 1;
+    kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k))
+
+    // chi-square test,卡方检验
+    kf->temp_matrix.numRows = kf->temp_vector1.numRows;
+    kf->temp_matrix.numCols = 1;
+    kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix);
+    // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k)))
+    kf->temp_vector.numRows = 1;
+    kf->temp_vector.numCols = kf->temp_vector1.numRows;
+    kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))'
+    kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare);
+    // rk is small,filter converged/converging
+    if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold)
+    {
+        QEKF_INS.ConvergeFlag = 1;
+    }
+    // rk is bigger than thre but once converged
+    if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
+    {
+        if (QEKF_INS.StableFlag)
+        {
+            QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验
+        }
+        else
+        {
+            QEKF_INS.ErrorCount = 0;
+        }
+
+        if (QEKF_INS.ErrorCount > 50)
+        {
+            // 滤波器发散
+            QEKF_INS.ConvergeFlag = 0;
+            kf->SkipEq5 = FALSE; // step-5 is cov mat P updating
+        }
+        else
+        {
+            //  残差未通过卡方检验 仅预测
+            //  xhat(k) = xhat'(k)
+            //  P(k) = P'(k)
+            memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
+            memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
+            kf->SkipEq5 = TRUE; // part5 is P updating
+            return;
+        }
+    }
+    else // if divergent or rk is not that big/acceptable,use adaptive gain
+    {
+        // scale adaptive,rk越小则增益越大,否则更相信预测值
+        if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
+        {
+            QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (
+                                             0.9f * QEKF_INS.ChiSquareTestThreshold);
+        }
+        else
+        {
+            QEKF_INS.AdaptiveGainScale = 1;
+        }
+        QEKF_INS.ErrorCount = 0;
+        kf->SkipEq5 = FALSE;
+    }
+
+    // cal kf-gain K
+    kf->temp_matrix.numRows = kf->Pminus.numRows;
+    kf->temp_matrix.numCols = kf->HT.numCols;
+    kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
+    kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
+
+    // implement adaptive
+    for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; ++i)
+    {
+        kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
+    }
+    for (uint8_t i = 4; i < 6; ++i)
+    {
+        for (uint8_t j = 0; j < 3; ++j)
+        {
+            kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
+        }
+    }
+
+    kf->temp_vector.numRows = kf->K.numRows;
+    kf->temp_vector.numCols = 1;
+    kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector);
+    // temp_vector = K(k)·(z(k) - H·xhat'(k))
+
+    // 零漂修正限幅,一般不会有过大的漂移
+    if (QEKF_INS.ConvergeFlag)
+    {
+        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->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
+}
+
+/**
+ * @brief EKF观测环节,其实就是把数据复制一下
+ *
+ * @param kf kf类型定义
+ */
+static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf)
+{
+    memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P));
+    memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K));
+    memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H));
+}
+
+/**
+ * @brief 自定义1/sqrt(x),速度更快
+ *
+ * @param x x
+ * @return float
+ */
+static float 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;
+}
diff --git a/User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.h b/User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.h
new file mode 100644
index 0000000..34d6a74
--- /dev/null
+++ b/User_Code/module/algorithm/QuaternionEKF/QuaternionEKF.h
@@ -0,0 +1,78 @@
+/**
+******************************************************************************
+ * @file    QuaternionEKF.h
+ * @author  Wang Hongxi
+ * @version V1.2.0
+ * @date    2022/3/8
+ * @brief   attitude update with gyro bias estimate and chi-square test
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#ifndef _QUAT_EKF_H
+#define _QUAT_EKF_H
+#include "kalman_filter.h"
+
+/* boolean type definitions */
+#ifndef TRUE
+#define TRUE 1 /**< boolean true  */
+#endif
+
+#ifndef FALSE
+#define FALSE 0 /**< boolean fails */
+#endif
+
+typedef struct
+{
+    uint8_t Initialized;
+    KalmanFilter_t IMU_QuaternionEKF;
+    uint8_t ConvergeFlag;
+    uint8_t StableFlag;
+    uint64_t ErrorCount;
+    uint64_t UpdateCount;
+
+    float q[4]; // 四元数估计值
+    float GyroBias[3]; // 陀螺仪零偏估计值
+
+    float Gyro[3];
+    float Accel[3];
+
+    float OrientationCosine[3];
+
+    float accLPFcoef;
+    float gyro_norm;
+    float accl_norm;
+    float AdaptiveGainScale;
+
+    float Roll;
+    float Pitch;
+    float Yaw;
+
+    float YawTotalAngle;
+
+    float Q1; // 四元数更新过程噪声
+    float Q2; // 陀螺仪零偏过程噪声
+    float R; // 加速度计量测噪声
+
+    float dt; // 姿态更新周期
+    mat ChiSquare;
+    float ChiSquare_Data[1]; // 卡方检验检测函数
+    float ChiSquareTestThreshold; // 卡方检验阈值
+    float lambda; // 渐消因子
+
+    int16_t YawRoundCount;
+
+    float YawAngleLast;
+} QEKF_INS_t;
+
+extern QEKF_INS_t QEKF_INS;
+extern float chiSquare;
+extern float ChiSquareTestThreshold;
+
+void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float process_noise2, float measure_noise,
+                            float lambda, float lpf);
+
+void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
+
+#endif
diff --git a/User_Code/module/algorithm/controller/controller.c b/User_Code/module/algorithm/controller/controller.c
new file mode 100644
index 0000000..8bebdc4
--- /dev/null
+++ b/User_Code/module/algorithm/controller/controller.c
@@ -0,0 +1,5 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#include "controller.h"
diff --git a/User_Code/module/algorithm/controller/controller.h b/User_Code/module/algorithm/controller/controller.h
new file mode 100644
index 0000000..bd25075
--- /dev/null
+++ b/User_Code/module/algorithm/controller/controller.h
@@ -0,0 +1,8 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#ifndef TRONONEH7_SCAFFOLD_CONTROLLER_H
+#define TRONONEH7_SCAFFOLD_CONTROLLER_H
+
+#endif //TRONONEH7_SCAFFOLD_CONTROLLER_H
diff --git a/User_Code/module/algorithm/crc/crc16.c b/User_Code/module/algorithm/crc/crc16.c
new file mode 100644
index 0000000..1a42885
--- /dev/null
+++ b/User_Code/module/algorithm/crc/crc16.c
@@ -0,0 +1,5 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#include "crc16.h"
diff --git a/User_Code/module/algorithm/crc/crc16.h b/User_Code/module/algorithm/crc/crc16.h
new file mode 100644
index 0000000..61f9162
--- /dev/null
+++ b/User_Code/module/algorithm/crc/crc16.h
@@ -0,0 +1,8 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#ifndef TRONONEH7_SCAFFOLD_CRC16_H
+#define TRONONEH7_SCAFFOLD_CRC16_H
+
+#endif //TRONONEH7_SCAFFOLD_CRC16_H
diff --git a/User_Code/module/algorithm/crc/crc8.c b/User_Code/module/algorithm/crc/crc8.c
new file mode 100644
index 0000000..0db8866
--- /dev/null
+++ b/User_Code/module/algorithm/crc/crc8.c
@@ -0,0 +1,5 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#include "crc8.h"
diff --git a/User_Code/module/algorithm/crc/crc8.h b/User_Code/module/algorithm/crc/crc8.h
new file mode 100644
index 0000000..c2e4544
--- /dev/null
+++ b/User_Code/module/algorithm/crc/crc8.h
@@ -0,0 +1,8 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#ifndef TRONONEH7_SCAFFOLD_CRC8_H
+#define TRONONEH7_SCAFFOLD_CRC8_H
+
+#endif //TRONONEH7_SCAFFOLD_CRC8_H
diff --git a/User_Code/module/algorithm/kalman_filter/kalman_filter.c b/User_Code/module/algorithm/kalman_filter/kalman_filter.c
new file mode 100644
index 0000000..256f9b1
--- /dev/null
+++ b/User_Code/module/algorithm/kalman_filter/kalman_filter.c
@@ -0,0 +1,5 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#include "kalman_filter.h"
diff --git a/User_Code/module/algorithm/kalman_filter/kalman_filter.h b/User_Code/module/algorithm/kalman_filter/kalman_filter.h
new file mode 100644
index 0000000..37881b7
--- /dev/null
+++ b/User_Code/module/algorithm/kalman_filter/kalman_filter.h
@@ -0,0 +1,8 @@
+//
+// Created by ASUS on 2025/11/15.
+//
+
+#ifndef TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
+#define TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
+
+#endif //TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
diff --git a/User_Code/module/algorithm/user_lib/user_lib.c b/User_Code/module/algorithm/user_lib/user_lib.c
new file mode 100644
index 0000000..042a896
--- /dev/null
+++ b/User_Code/module/algorithm/user_lib/user_lib.c
@@ -0,0 +1,215 @@
+/**
+ ******************************************************************************
+ * @file	 user_lib.c
+ * @author  Wang Hongxi
+ * @author  modified by neozng
+ * @version 0.2 beta
+ * @date    2021/2/18
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#include "stdlib.h"
+#include "memory.h"
+#include "user_lib.h"
+#include "math.h"
+#include "main.h"
+
+#ifdef _CMSIS_OS_H
+#define user_malloc pvPortMalloc
+#else
+#define user_malloc malloc
+#endif
+
+void *zmalloc(size_t size)
+{
+    void *ptr = malloc(size);
+    memset(ptr, 0, size);
+    return ptr;
+}
+
+// 快速开方
+float Sqrt(float x)
+{
+    float y;
+    float delta;
+    float maxError;
+
+    if (x <= 0)
+    {
+        return 0;
+    }
+
+    // initial guess
+    y = x / 2;
+
+    // refine
+    maxError = x * 0.001f;
+
+    do
+    {
+        delta = (y * y) - x;
+        y -= delta / (2 * y);
+    }
+    while (delta > maxError || delta < -maxError);
+
+    return y;
+}
+
+// 绝对值限制
+float abs_limit(float num, float Limit)
+{
+    if (num > Limit)
+    {
+        num = Limit;
+    }
+    else if (num < -Limit)
+    {
+        num = -Limit;
+    }
+    return num;
+}
+
+// 判断符号位
+float sign(float value)
+{
+    if (value >= 0.0f)
+    {
+        return 1.0f;
+    }
+    else
+    {
+        return -1.0f;
+    }
+}
+
+// 浮点死区
+float float_deadband(float Value, float minValue, float maxValue)
+{
+    if (Value < maxValue && Value > minValue)
+    {
+        Value = 0.0f;
+    }
+    return Value;
+}
+
+// 限幅函数
+float float_constrain(float Value, float minValue, float maxValue)
+{
+    if (Value < minValue)
+        return minValue;
+    else if (Value > maxValue)
+        return maxValue;
+    else
+        return Value;
+}
+
+// 限幅函数
+int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
+{
+    if (Value < minValue)
+        return minValue;
+    else if (Value > maxValue)
+        return maxValue;
+    else
+        return Value;
+}
+
+// 循环限幅函数
+float loop_float_constrain(float Input, float minValue, float maxValue)
+{
+    if (maxValue < minValue)
+    {
+        return Input;
+    }
+
+    if (Input > maxValue)
+    {
+        float len = maxValue - minValue;
+        while (Input > maxValue)
+        {
+            Input -= len;
+        }
+    }
+    else if (Input < minValue)
+    {
+        float len = maxValue - minValue;
+        while (Input < minValue)
+        {
+            Input += len;
+        }
+    }
+    return Input;
+}
+
+// 弧度格式化为-PI~PI
+
+// 角度格式化为-180~180
+float theta_format(float Ang)
+{
+    return loop_float_constrain(Ang, -180.0f, 180.0f);
+}
+
+int float_rounding(float raw)
+{
+    static int integer;
+    static float decimal;
+    integer = (int) raw;
+    decimal = raw - integer;
+    if (decimal > 0.5f)
+        integer++;
+    return integer;
+}
+
+// 三维向量归一化
+float *Norm3d(float *v)
+{
+    float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+    v[0] /= len;
+    v[1] /= len;
+    v[2] /= len;
+    return v;
+}
+
+// 计算模长
+float NormOf3d(float *v)
+{
+    return Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+}
+
+// 三维向量叉乘v1 x v2
+void Cross3d(float *v1, float *v2, float *res)
+{
+    res[0] = v1[1] * v2[2] - v1[2] * v2[1];
+    res[1] = v1[2] * v2[0] - v1[0] * v2[2];
+    res[2] = v1[0] * v2[1] - v1[1] * v2[0];
+}
+
+// 三维向量点乘
+float Dot3d(float *v1, float *v2)
+{
+    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+}
+
+// 均值滤波,删除buffer中的最后一个元素,填入新的元素并求平均值
+float AverageFilter(float new_data, float *buf, uint8_t len)
+{
+    float sum = 0;
+    for (uint8_t i = 0; i < len - 1; i++)
+    {
+        buf[i] = buf[i + 1];
+        sum += buf[i];
+    }
+    buf[len - 1] = new_data;
+    sum += new_data;
+    return sum / len;
+}
+
+void MatInit(mat *m, uint8_t row, uint8_t col)
+{
+    m->numCols = col;
+    m->numRows = row;
+    m->pData = (float *) zmalloc(row * col * sizeof(float));
+}
diff --git a/User_Code/module/algorithm/user_lib/user_lib.h b/User_Code/module/algorithm/user_lib/user_lib.h
new file mode 100644
index 0000000..22f5896
--- /dev/null
+++ b/User_Code/module/algorithm/user_lib/user_lib.h
@@ -0,0 +1,165 @@
+/**
+ ******************************************************************************
+ * @file	 user_lib.h
+ * @author  Wang Hongxi
+ * @version V1.0.0
+ * @date    2021/2/18
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#ifndef _USER_LIB_H
+#define _USER_LIB_H
+#include "stdint.h"
+#include "main.h"
+#include "cmsis_os.h"
+
+enum
+{
+    CHASSIS_DEBUG = 1,
+    GIMBAL_DEBUG,
+    INS_DEBUG,
+    RC_DEBUG,
+    IMU_HEAT_DEBUG,
+    SHOOT_DEBUG,
+    AIMASSIST_DEBUG,
+};
+
+extern uint8_t GlobalDebugMode;
+
+#ifndef user_malloc
+#ifdef _CMSIS_OS_H
+#define user_malloc pvPortMalloc
+#else
+#define user_malloc malloc
+#endif
+#endif
+
+/* boolean type definitions */
+#ifndef TRUE
+#define TRUE 1 /**< boolean true  */
+#endif
+
+#ifndef FALSE
+#define FALSE 0 /**< boolean fails */
+#endif
+
+/* math relevant */
+/* radian coefficient */
+#ifndef RADIAN_COEF
+#define RADIAN_COEF 57.295779513f
+#endif
+
+/* circumference ratio */
+#ifndef PI
+#define PI 3.14159265354f
+#endif
+
+#define VAL_LIMIT(val, min, max) \
+    do                           \
+    {                            \
+        if ((val) <= (min))      \
+        {                        \
+            (val) = (min);       \
+        }                        \
+        else if ((val) >= (max)) \
+        {                        \
+            (val) = (max);       \
+        }                        \
+    } while (0)
+
+#define ANGLE_LIMIT_360(val, angle)     \
+    do                                  \
+    {                                   \
+        (val) = (angle) - (int)(angle); \
+        (val) += (int)(angle) % 360;    \
+    } while (0)
+
+#define ANGLE_LIMIT_360_TO_180(val) \
+    do                              \
+    {                               \
+        if ((val) > 180)            \
+            (val) -= 360;           \
+    } while (0)
+
+#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
+#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
+
+typedef struct
+{
+    float input; //输入数据
+    float out; //输出数据
+    float min_value; //限幅最小值
+    float max_value; //限幅最大值
+    float frame_period; //时间间隔
+} ramp_function_source_t;
+
+typedef __packed struct
+{
+    uint16_t Order;
+    uint32_t Count;
+
+    float *x;
+    float *y;
+
+    float k;
+    float b;
+
+    float StandardDeviation;
+
+    float t[4];
+} Ordinary_Least_Squares_t;
+
+//快速开方
+float Sqrt(float x);
+
+//斜波函数初始化
+void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
+
+//斜波函数计算
+float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
+
+//绝对限制
+float abs_limit(float num, float Limit);
+
+//判断符号位
+float sign(float value);
+
+//浮点死区
+float float_deadband(float Value, float minValue, float maxValue);
+
+// int26死区
+int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
+
+//限幅函数
+float float_constrain(float Value, float minValue, float maxValue);
+
+//限幅函数
+int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
+
+//循环限幅函数
+float loop_float_constrain(float Input, float minValue, float maxValue);
+
+//角度 °限幅 180 ~ -180
+float theta_format(float Ang);
+
+int float_rounding(float raw);
+
+//弧度格式化为-PI~PI
+#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
+
+void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order);
+
+void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y);
+
+float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y);
+
+float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y);
+
+float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS);
+
+float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS);
+
+#endif
diff --git a/User_Code/module/periph/imu/BMI088Middleware.c b/User_Code/module/periph/imu/BMI088Middleware.c
new file mode 100644
index 0000000..5343219
--- /dev/null
+++ b/User_Code/module/periph/imu/BMI088Middleware.c
@@ -0,0 +1,31 @@
+#include "BMI088Middleware.h"
+#include "main.h"
+
+SPI_HandleTypeDef *BMI088_SPI;
+
+void BMI088_ACCEL_NS_L(void)
+{
+    HAL_GPIO_WritePin(ACC_CS_GPIO_Port, ACC_CS_Pin, GPIO_PIN_RESET);
+}
+
+void BMI088_ACCEL_NS_H(void)
+{
+    HAL_GPIO_WritePin(ACC_CS_GPIO_Port, ACC_CS_Pin, GPIO_PIN_SET);
+}
+
+void BMI088_GYRO_NS_L(void)
+{
+    HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_RESET);
+}
+
+void BMI088_GYRO_NS_H(void)
+{
+    HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
+}
+
+uint8_t BMI088_read_write_byte(uint8_t txdata)
+{
+    uint8_t rx_data;
+    HAL_SPI_TransmitReceive(BMI088_SPI, &txdata, &rx_data, 1, 100); //1000or100?
+    return rx_data;
+}
diff --git a/User_Code/module/periph/imu/BMI088Middleware.h b/User_Code/module/periph/imu/BMI088Middleware.h
new file mode 100644
index 0000000..7e2754b
--- /dev/null
+++ b/User_Code/module/periph/imu/BMI088Middleware.h
@@ -0,0 +1,34 @@
+#ifndef BMI088MIDDLEWARE_H
+#define BMI088MIDDLEWARE_H
+
+#include "main.h"
+
+#define BMI088_USE_SPI
+//#define BMI088_USE_IIC
+
+/*
+#define CS1_ACCEL_GPIO_Port ACCEL_NSS_GPIO_Port
+#define CS1_ACCEL_Pin ACCEL_NSS_Pin
+#define CS1_GYRO_GPIO_Port GYRO_NSS_GPIO_Port
+#define CS1_GYRO_Pin GYRO_NSS_Pin
+*/
+
+
+#if defined(BMI088_USE_SPI)
+extern void BMI088_ACCEL_NS_L(void);
+
+extern void BMI088_ACCEL_NS_H(void);
+
+extern void BMI088_GYRO_NS_L(void);
+
+extern void BMI088_GYRO_NS_H(void);
+
+extern uint8_t BMI088_read_write_byte(uint8_t reg);
+
+extern SPI_HandleTypeDef *BMI088_SPI;
+
+#elif defined(BMI088_USE_IIC)
+
+#endif
+
+#endif
diff --git a/User_Code/module/periph/imu/BMI088driver.c b/User_Code/module/periph/imu/BMI088driver.c
new file mode 100644
index 0000000..e4ae5eb
--- /dev/null
+++ b/User_Code/module/periph/imu/BMI088driver.c
@@ -0,0 +1,416 @@
+#include "BMI088driver.h"
+#include "BMI088reg.h"
+#include "BMI088Middleware.h"
+#include "bsp_dwt.h"
+#include "bsp_log.h"
+#include <math.h>
+
+#pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
+
+float BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN;
+float BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN;
+
+static uint8_t res = 0;
+static uint8_t write_reg_num = 0;
+static uint8_t error = BMI088_NO_ERROR;
+float gyroDiff[3], gNormDiff;
+
+uint8_t caliOffset = 1;
+int16_t caliCount = 0;
+
+IMU_Data_t BMI088;
+
+#if defined(BMI088_USE_SPI)
+
+#define BMI088_accel_write_single_reg(reg, data) \
+    {                                            \
+        BMI088_ACCEL_NS_L();                     \
+        BMI088_write_single_reg((reg), (data));  \
+        BMI088_ACCEL_NS_H();                     \
+    }
+#define BMI088_accel_read_single_reg(reg, data) \
+    {                                           \
+        BMI088_ACCEL_NS_L();                    \
+        BMI088_read_write_byte((reg) | 0x80);   \
+        BMI088_read_write_byte(0x55);           \
+        (data) = BMI088_read_write_byte(0x55);  \
+        BMI088_ACCEL_NS_H();                    \
+    }
+#define BMI088_accel_read_muli_reg(reg, data, len) \
+    {                                              \
+        BMI088_ACCEL_NS_L();                       \
+        BMI088_read_write_byte((reg) | 0x80);      \
+        BMI088_read_muli_reg(reg, data, len);      \
+        BMI088_ACCEL_NS_H();                       \
+    }
+
+#define BMI088_gyro_write_single_reg(reg, data) \
+    {                                           \
+        BMI088_GYRO_NS_L();                     \
+        BMI088_write_single_reg((reg), (data)); \
+        BMI088_GYRO_NS_H();                     \
+    }
+#define BMI088_gyro_read_single_reg(reg, data)  \
+    {                                           \
+        BMI088_GYRO_NS_L();                     \
+        BMI088_read_single_reg((reg), &(data)); \
+        BMI088_GYRO_NS_H();                     \
+    }
+#define BMI088_gyro_read_muli_reg(reg, data, len)   \
+    {                                               \
+        BMI088_GYRO_NS_L();                         \
+        BMI088_read_muli_reg((reg), (data), (len)); \
+        BMI088_GYRO_NS_H();                         \
+    }
+
+static void BMI088_write_single_reg(uint8_t reg, uint8_t data);
+static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data);
+static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len);
+
+#elif defined(BMI088_USE_IIC)
+#endif
+
+static uint8_t BMI088_Accel_Init_Table[BMI088_WRITE_ACCEL_REG_NUM][3] =
+{
+        {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
+        {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
+        {BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_1600_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
+        {BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
+        {BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW,
+         BMI088_INT1_IO_CTRL_ERROR},
+        {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}
+
+};
+
+static uint8_t BMI088_Gyro_Init_Table[BMI088_WRITE_GYRO_REG_NUM][3] =
+{
+        {BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
+        {BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
+        {BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
+        {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
+        {BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW,
+         BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
+        {BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}
+
+};
+
+static void Calibrate_MPU_Offset(IMU_Data_t *bmi088);
+
+uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate)
+{
+    BMI088_SPI = bmi088_SPI;
+    error = BMI088_NO_ERROR;
+
+    error |= bmi088_accel_init();
+    error |= bmi088_gyro_init();
+    if (calibrate)
+        Calibrate_MPU_Offset(&BMI088);
+    else
+    {
+        BMI088.GyroOffset[0] = GxOFFSET;
+        BMI088.GyroOffset[1] = GyOFFSET;
+        BMI088.GyroOffset[2] = GzOFFSET;
+        BMI088.gNorm = gNORM;
+        BMI088.AccelScale = 9.81f / BMI088.gNorm;
+        BMI088.TempWhenCali = 40;
+    }
+
+    return error;
+}
+
+void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
+{
+    static float startTime;
+    static uint16_t CaliTimes = 6000;
+    uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
+    int16_t bmi088_raw_temp;
+    float gyroMax[3], gyroMin[3];
+    float gNormTemp = 0.0f, gNormMax = 0.0f, gNormMin = 0.0f;
+
+    startTime = DWT_GetTimeline_s();
+    do
+    {
+        if (DWT_GetTimeline_s() - startTime > 12)
+        {
+            // ��????
+            bmi088->GyroOffset[0] = GxOFFSET;
+            bmi088->GyroOffset[1] = GyOFFSET;
+            bmi088->GyroOffset[2] = GzOFFSET;
+            bmi088->gNorm = gNORM;
+            bmi088->TempWhenCali = 40;
+            LOGERROR("[BMI088] Calibrate Failed! Use offline params");
+            break;
+        }
+
+        DWT_Delay(0.005);
+        bmi088->gNorm = 0;
+        bmi088->GyroOffset[0] = 0;
+        bmi088->GyroOffset[1] = 0;
+        bmi088->GyroOffset[2] = 0;
+
+        for (uint16_t i = 0; i < CaliTimes; ++i)
+        {
+            BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
+            bmi088_raw_temp = (int16_t) ((buf[1]) << 8) | buf[0];
+            bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN;
+            bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
+            bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN;
+            bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
+            bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN;
+            gNormTemp = sqrtf(bmi088->Accel[0] * bmi088->Accel[0] +
+                              bmi088->Accel[1] * bmi088->Accel[1] +
+                              bmi088->Accel[2] * bmi088->Accel[2]);
+            bmi088->gNorm += gNormTemp;
+
+            BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
+            if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
+            {
+                bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
+                bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
+                bmi088->GyroOffset[0] += bmi088->Gyro[0];
+                bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
+                bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
+                bmi088->GyroOffset[1] += bmi088->Gyro[1];
+                bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
+                bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
+                bmi088->GyroOffset[2] += bmi088->Gyro[2];
+            }
+
+            if (i == 0)
+            {
+                gNormMax = gNormTemp;
+                gNormMin = gNormTemp;
+                for (uint8_t j = 0; j < 3; ++j)
+                {
+                    gyroMax[j] = bmi088->Gyro[j];
+                    gyroMin[j] = bmi088->Gyro[j];
+                }
+            }
+            else
+            {
+                if (gNormTemp > gNormMax)
+                    gNormMax = gNormTemp;
+                if (gNormTemp < gNormMin)
+                    gNormMin = gNormTemp;
+                for (uint8_t j = 0; j < 3; ++j)
+                {
+                    if (bmi088->Gyro[j] > gyroMax[j])
+                        gyroMax[j] = bmi088->Gyro[j];
+                    if (bmi088->Gyro[j] < gyroMin[j])
+                        gyroMin[j] = bmi088->Gyro[j];
+                }
+            }
+
+            gNormDiff = gNormMax - gNormMin;
+            for (uint8_t j = 0; j < 3; ++j)
+                gyroDiff[j] = gyroMax[j] - gyroMin[j];
+            if (gNormDiff > 0.5f ||
+                gyroDiff[0] > 0.5f || //0.15
+                gyroDiff[1] > 0.5f || //0.15
+                gyroDiff[2] > 0.5f) //0.15
+            {
+                LOGWARNING("[bmi088] calibration was interrupted\n");
+                break;
+            }
+
+            DWT_Delay(0.0005);
+        }
+
+        bmi088->gNorm /= (float) CaliTimes;
+        for (uint8_t i = 0; i < 3; ++i)
+            bmi088->GyroOffset[i] /= (float) CaliTimes;
+
+        BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
+        bmi088_raw_temp = (int16_t) ((buf[0] << 3) | (buf[1] >> 5));
+        if (bmi088_raw_temp > 1023)
+            bmi088_raw_temp -= 2048;
+        bmi088->TempWhenCali = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
+
+        caliCount++;
+    }
+    while (gNormDiff > 0.5f ||
+           fabsf(bmi088->gNorm - 9.8f) > 0.5f ||
+           gyroDiff[0] > 0.15f ||
+           gyroDiff[1] > 0.15f ||
+           gyroDiff[2] > 0.15f ||
+           fabsf(bmi088->GyroOffset[0]) > 0.01f ||
+           fabsf(bmi088->GyroOffset[1]) > 0.01f ||
+           fabsf(bmi088->GyroOffset[2]) > 0.01f);
+
+    bmi088->AccelScale = 9.81f / bmi088->gNorm;
+}
+
+uint8_t bmi088_accel_init(void)
+{
+    // check commiunication
+    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
+    // DWT_Delay(0.001);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
+    // DWT_Delay(0.001);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+    // accel software reset
+    BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
+    // HAL_Delay(BMI088_LONG_DELAY_TIME);
+    HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
+    // check commiunication is normal after reset
+    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
+    // DWT_Delay(0.001);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
+    // DWT_Delay(0.001);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+
+    // check the "who am I"
+    if (res != BMI088_ACC_CHIP_ID_VALUE)
+    {
+        LOGERROR("[bmi088] Can not read bmi088 acc chip id");
+        return BMI088_NO_SENSOR;
+    }
+
+    // set accel sonsor config and check
+    for (write_reg_num = 0; write_reg_num < BMI088_WRITE_ACCEL_REG_NUM; write_reg_num++)
+    {
+
+        BMI088_accel_write_single_reg(BMI088_Accel_Init_Table[write_reg_num][0],
+                                      BMI088_Accel_Init_Table[write_reg_num][1]);
+        // DWT_Delay(0.001);
+        HAL_Delay(BMI088_LONG_DELAY_TIME);
+
+        BMI088_accel_read_single_reg(BMI088_Accel_Init_Table[write_reg_num][0], res);
+        // DWT_Delay(0.001);
+        HAL_Delay(BMI088_LONG_DELAY_TIME);
+
+        if (res != BMI088_Accel_Init_Table[write_reg_num][1])
+        {
+            // write_reg_num--;
+            // return BMI088_Accel_Init_Table[write_reg_num][2];
+            error |= BMI088_Accel_Init_Table[write_reg_num][2];
+        }
+    }
+    return BMI088_NO_ERROR;
+}
+
+uint8_t bmi088_gyro_init(void)
+{
+    // check commiunication
+    BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+    BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+
+    // reset the gyro sensor
+    BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
+    // HAL_Delay(BMI088_LONG_DELAY_TIME);
+    HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
+    // check commiunication is normal after reset
+    BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+    BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
+    HAL_Delay(BMI088_LONG_DELAY_TIME);
+
+    // check the "who am I"
+    if (res != BMI088_GYRO_CHIP_ID_VALUE)
+    {
+        LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
+        return BMI088_NO_SENSOR;
+    }
+
+    // set gyro sonsor config and check
+    for (write_reg_num = 0; write_reg_num < BMI088_WRITE_GYRO_REG_NUM; write_reg_num++)
+    {
+
+        BMI088_gyro_write_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0],
+                                     BMI088_Gyro_Init_Table[write_reg_num][1]);
+        HAL_Delay(BMI088_LONG_DELAY_TIME);
+
+        BMI088_gyro_read_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0], res);
+        HAL_Delay(BMI088_LONG_DELAY_TIME); //但是不知道为什么还是yaw pitch还是nan
+
+        if (res != BMI088_Gyro_Init_Table[write_reg_num][1])
+        {
+            write_reg_num--;
+            // return BMI088_Gyro_Init_Table[write_reg_num][2];
+            error |= BMI088_Accel_Init_Table[write_reg_num][2];
+        }
+    }
+
+    return BMI088_NO_ERROR;
+}
+
+void BMI088_Read(IMU_Data_t *bmi088)
+{
+    static uint8_t buf[8] = {0};
+    static int16_t bmi088_raw_temp;
+
+    BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
+
+    bmi088_raw_temp = (int16_t) ((buf[1]) << 8) | buf[0];
+    bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
+    bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
+    bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
+    bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
+    bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
+
+    BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
+    if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
+    {
+        if (caliOffset)
+        {
+            bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
+            bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[0];
+            bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
+            bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1];
+            bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
+            bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2];
+        }
+        else
+        {
+            bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
+            bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
+            bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
+            bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
+            bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
+            bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
+        }
+    }
+    BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
+
+    bmi088_raw_temp = (int16_t) ((buf[0] << 3) | (buf[1] >> 5));
+
+    if (bmi088_raw_temp > 1023)
+    {
+        bmi088_raw_temp -= 2048;
+    }
+
+    bmi088->Temperature = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
+}
+
+#if defined(BMI088_USE_SPI)
+
+static void BMI088_write_single_reg(uint8_t reg, uint8_t data)
+{
+    BMI088_read_write_byte(reg);
+    BMI088_read_write_byte(data);
+}
+
+static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data)
+{
+    BMI088_read_write_byte(reg | 0x80);
+    *return_data = BMI088_read_write_byte(0x55);
+}
+
+static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len)
+{
+    BMI088_read_write_byte(reg | 0x80);
+
+    while (len != 0)
+    {
+        *buf = BMI088_read_write_byte(0x55);
+        buf++;
+        len--;
+    }
+}
+#elif defined(BMI088_USE_IIC)
+
+#endif
diff --git a/User_Code/module/periph/imu/BMI088driver.h b/User_Code/module/periph/imu/BMI088driver.h
new file mode 100644
index 0000000..52073a4
--- /dev/null
+++ b/User_Code/module/periph/imu/BMI088driver.h
@@ -0,0 +1,133 @@
+#ifndef BMI088DRIVER_H
+#define BMI088DRIVER_H
+
+#include "stdint.h"
+#include "main.h"
+
+#define BMI088_TEMP_FACTOR 0.125f
+#define BMI088_TEMP_OFFSET 23.0f
+
+#define BMI088_WRITE_ACCEL_REG_NUM 6
+#define BMI088_WRITE_GYRO_REG_NUM 6
+
+#define BMI088_GYRO_DATA_READY_BIT 0
+#define BMI088_ACCEL_DATA_READY_BIT 1
+#define BMI088_ACCEL_TEMP_DATA_READY_BIT 2
+
+#define BMI088_LONG_DELAY_TIME 80
+#define BMI088_COM_WAIT_SENSOR_TIME 150
+
+#define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
+#define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)
+
+#define BMI088_ACCEL_3G_SEN 0.0008974358974f
+#define BMI088_ACCEL_6G_SEN 0.00179443359375f
+#define BMI088_ACCEL_12G_SEN 0.0035888671875f
+#define BMI088_ACCEL_24G_SEN 0.007177734375f
+
+#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
+#define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
+#define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
+#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
+#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
+
+// ���ֶ��޸�
+#if INFANTRY_ID == 0
+#define GxOFFSET 0.00247530174f
+#define GyOFFSET 0.000393082853f
+#define GzOFFSET 0.000393082853f
+#define gNORM 9.69293118f
+#elif INFANTRY_ID == 1
+#define GxOFFSET 0.0007222f
+#define GyOFFSET -0.001786f
+#define GzOFFSET 0.0004346f
+#define gNORM 9.876785f
+#elif INFANTRY_ID == 2
+#define GxOFFSET 0.0007222f
+#define GyOFFSET -0.001786f
+#define GzOFFSET 0.0004346f
+#define gNORM 9.876785f
+#elif INFANTRY_ID == 3
+#define GxOFFSET 0.00270364084f
+#define GyOFFSET -0.000532632112f
+#define GzOFFSET 0.00478090625f
+#define gNORM 9.73574924f
+#elif INFANTRY_ID == 4
+#define GxOFFSET 0.0007222f
+#define GyOFFSET -0.001786f
+#define GzOFFSET 0.0004346f
+#define gNORM 9.876785f
+#endif
+
+/* IMU数据结构体 */
+typedef struct
+{
+    float Accel[3];
+
+    float Gyro[3];
+
+    float TempWhenCali;
+    float Temperature;
+
+    float AccelScale;
+    float GyroOffset[3];
+
+    float gNorm;
+} IMU_Data_t;
+
+/* BMI088错误码枚举 */
+enum
+{
+    BMI088_NO_ERROR = 0x00,
+    BMI088_ACC_PWR_CTRL_ERROR = 0x01,
+    BMI088_ACC_PWR_CONF_ERROR = 0x02,
+    BMI088_ACC_CONF_ERROR = 0x03,
+    BMI088_ACC_SELF_TEST_ERROR = 0x04,
+    BMI088_ACC_RANGE_ERROR = 0x05,
+    BMI088_INT1_IO_CTRL_ERROR = 0x06,
+    BMI088_INT_MAP_DATA_ERROR = 0x07,
+    BMI088_GYRO_RANGE_ERROR = 0x08,
+    BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
+    BMI088_GYRO_LPM1_ERROR = 0x0A,
+    BMI088_GYRO_CTRL_ERROR = 0x0B,
+    BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
+    BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,
+
+    BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
+    BMI088_SELF_TEST_GYRO_ERROR = 0x40,
+    BMI088_NO_SENSOR = 0xFF,
+};
+
+extern IMU_Data_t BMI088;
+
+/**
+ * @brief 初始化BMI088,传入连接的SPI总线handle,以及是否进行在线标定
+ *
+ * @param bmi088_SPI handle
+ * @param calibrate  1为进行在线标定,0使用离线数据
+ * @return uint8_t   成功则返回BMI088_NO_ERROR
+ */
+extern uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
+
+/**
+ * @brief 加速计初始化
+ *
+ * @return uint8_t
+ */
+extern uint8_t bmi088_accel_init(void);
+
+/**
+ * @brief 陀螺仪初始化
+ *
+ * @return uint8_t
+ */
+extern uint8_t bmi088_gyro_init(void);
+
+/**
+ * @brief 读取一次BMI088的数据,包括gyro和accel
+ *
+ * @param bmi088 传入BMI088实例(结构体)
+ */
+extern void BMI088_Read(IMU_Data_t *bmi088);
+
+#endif
diff --git a/User_Code/module/periph/imu/BMI088reg.h b/User_Code/module/periph/imu/BMI088reg.h
new file mode 100644
index 0000000..f571f7e
--- /dev/null
+++ b/User_Code/module/periph/imu/BMI088reg.h
@@ -0,0 +1,180 @@
+#ifndef BMI088REG_H
+#define BMI088REG_H
+
+#define BMI088_ACC_CHIP_ID 0x00 // the register is  " Who am I "
+#define BMI088_ACC_CHIP_ID_VALUE 0x1E
+
+#define BMI088_ACC_ERR_REG 0x02
+#define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
+#define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
+#define BMI088_FATAL_ERROR_SHFITS 0x0
+#define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)
+
+#define BMI088_ACC_STATUS 0x03
+#define BMI088_ACCEL_DRDY_SHFITS 0x7
+#define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)
+
+#define BMI088_ACCEL_XOUT_L 0x12
+#define BMI088_ACCEL_XOUT_M 0x13
+#define BMI088_ACCEL_YOUT_L 0x14
+#define BMI088_ACCEL_YOUT_M 0x15
+#define BMI088_ACCEL_ZOUT_L 0x16
+#define BMI088_ACCEL_ZOUT_M 0x17
+
+#define BMI088_SENSORTIME_DATA_L 0x18
+#define BMI088_SENSORTIME_DATA_M 0x19
+#define BMI088_SENSORTIME_DATA_H 0x1A
+
+#define BMI088_ACC_INT_STAT_1 0x1D
+#define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
+#define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)
+
+#define BMI088_TEMP_M 0x22
+
+#define BMI088_TEMP_L 0x23
+
+#define BMI088_ACC_CONF 0x40
+#define BMI088_ACC_CONF_MUST_Set 0x80
+#define BMI088_ACC_BWP_SHFITS 0x4
+#define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
+#define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
+#define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)
+
+#define BMI088_ACC_ODR_SHFITS 0x0
+#define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
+#define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)
+
+#define BMI088_ACC_RANGE 0x41
+
+#define BMI088_ACC_RANGE_SHFITS 0x0
+#define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
+#define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
+#define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
+#define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)
+
+#define BMI088_INT1_IO_CTRL 0x53
+#define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
+#define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
+#define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
+#define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
+#define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
+#define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
+#define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
+#define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
+
+#define BMI088_INT2_IO_CTRL 0x54
+#define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
+#define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
+#define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
+#define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
+#define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
+#define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
+#define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
+#define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
+
+#define BMI088_INT_MAP_DATA 0x58
+#define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
+#define BMI088_ACC_INT2_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
+#define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
+#define BMI088_ACC_INT1_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)
+
+#define BMI088_ACC_SELF_TEST 0x6D
+#define BMI088_ACC_SELF_TEST_OFF 0x00
+#define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
+#define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09
+
+#define BMI088_ACC_PWR_CONF 0x7C
+#define BMI088_ACC_PWR_SUSPEND_MODE 0x03
+#define BMI088_ACC_PWR_ACTIVE_MODE 0x00
+
+#define BMI088_ACC_PWR_CTRL 0x7D
+#define BMI088_ACC_ENABLE_ACC_OFF 0x00
+#define BMI088_ACC_ENABLE_ACC_ON 0x04
+
+#define BMI088_ACC_SOFTRESET 0x7E
+#define BMI088_ACC_SOFTRESET_VALUE 0xB6
+
+#define BMI088_GYRO_CHIP_ID 0x00
+#define BMI088_GYRO_CHIP_ID_VALUE 0x0F
+
+#define BMI088_GYRO_X_L 0x02
+#define BMI088_GYRO_X_H 0x03
+#define BMI088_GYRO_Y_L 0x04
+#define BMI088_GYRO_Y_H 0x05
+#define BMI088_GYRO_Z_L 0x06
+#define BMI088_GYRO_Z_H 0x07
+
+#define BMI088_GYRO_INT_STAT_1 0x0A
+#define BMI088_GYRO_DYDR_SHFITS 0x7
+#define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)
+
+#define BMI088_GYRO_RANGE 0x0F
+#define BMI088_GYRO_RANGE_SHFITS 0x0
+#define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
+#define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
+#define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
+#define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
+#define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)
+
+#define BMI088_GYRO_BANDWIDTH 0x10
+// the first num means Output data  rate, the second num means bandwidth
+#define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
+#define BMI088_GYRO_2000_532_HZ 0x00
+#define BMI088_GYRO_2000_230_HZ 0x01
+#define BMI088_GYRO_1000_116_HZ 0x02
+#define BMI088_GYRO_400_47_HZ 0x03
+#define BMI088_GYRO_200_23_HZ 0x04
+#define BMI088_GYRO_100_12_HZ 0x05
+#define BMI088_GYRO_200_64_HZ 0x06
+#define BMI088_GYRO_100_32_HZ 0x07
+
+#define BMI088_GYRO_LPM1 0x11
+#define BMI088_GYRO_NORMAL_MODE 0x00
+#define BMI088_GYRO_SUSPEND_MODE 0x80
+#define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20
+
+#define BMI088_GYRO_SOFTRESET 0x14
+#define BMI088_GYRO_SOFTRESET_VALUE 0xB6
+
+#define BMI088_GYRO_CTRL 0x15
+#define BMI088_DRDY_OFF 0x00
+#define BMI088_DRDY_ON 0x80
+
+#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
+#define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
+#define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
+#define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
+#define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
+#define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
+#define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
+#define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
+#define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
+#define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
+#define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
+#define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
+#define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
+
+#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18
+
+#define BMI088_GYRO_DRDY_IO_OFF 0x00
+#define BMI088_GYRO_DRDY_IO_INT3 0x01
+#define BMI088_GYRO_DRDY_IO_INT4 0x80
+#define BMI088_GYRO_DRDY_IO_BOTH (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)
+
+#define BMI088_GYRO_SELF_TEST 0x3C
+#define BMI088_GYRO_RATE_OK_SHFITS 0x4
+#define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
+#define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
+#define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
+#define BMI088_GYRO_BIST_RDY_SHFITS 0x1
+#define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
+#define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
+#define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)
+
+#endif
diff --git a/User_Code/module/periph/imu/ins_task.c b/User_Code/module/periph/imu/ins_task.c
new file mode 100644
index 0000000..56bec96
--- /dev/null
+++ b/User_Code/module/periph/imu/ins_task.c
@@ -0,0 +1,351 @@
+/**
+ ******************************************************************************
+ * @file    ins_task.c
+ * @author  Wang Hongxi
+ * @author  annotation and modificaiton by neozng
+ * @version V2.0.0
+ * @date    2022/2/23
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#include "ins_task.h"
+#include "controller.h"
+#include "QuaternionEKF.h"
+#include "spi.h"
+#include "tim.h"
+#include "user_lib.h"
+#include "general_def.h"
+// #include "master_process.h"
+
+static INS_t INS;
+static IMU_Param_t IMU_Param;
+static PIDInstance TempCtrl = {0};
+
+//旋转矩阵
+const float xb[3] = {1, 0, 0};
+const float yb[3] = {0, 1, 0};
+const float zb[3] = {0, 0, 1};
+
+// 用于获取两次采样之间的时间间隔
+static uint32_t INS_DWT_Count = 0;
+static float dt = 0, t = 0;
+static float RefTemp = 40; // 恒温设定温度
+
+static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
+
+static void IMUPWMSet(uint16_t pwm)
+{
+    __HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, pwm);
+}
+
+/**
+ * @brief 温度控制
+ *
+ */
+static void IMU_Temperature_Ctrl(void)
+{
+    PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
+    IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
+}
+
+// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
+static void InitQuaternion(float *init_q4)
+{
+    float acc_init[3] = {0};
+    float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
+    float axis_rot[3] = {0}; // 旋转轴
+    // 读取100次加速度计数据,取平均值作为初始值
+    for (uint8_t i = 0; i < 100; ++i)
+    {
+        BMI088_Read(&BMI088);
+        acc_init[X] += BMI088.Accel[X];
+        acc_init[Y] += BMI088.Accel[Y];
+        acc_init[Z] += BMI088.Accel[Z];
+        DWT_Delay(0.001);
+    }
+    for (uint8_t i = 0; i < 3; ++i)
+        acc_init[i] /= 100;
+    Norm3d(acc_init);
+    // 计算原始加速度矢量和导航系重力加速度矢量的夹角
+    float angle = acosf(Dot3d(acc_init, gravity_norm));
+    Cross3d(acc_init, gravity_norm, axis_rot);
+    Norm3d(axis_rot);
+    init_q4[0] = cosf(angle / 2.0f);
+    for (uint8_t i = 0; i < 2; ++i)
+        init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
+}
+
+attitude_t *INS_Init(void)
+{
+    if (!INS.init)
+        INS.init = 1;
+    else
+        return (attitude_t *) &INS.Gyro;
+
+    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
+
+    while (BMI088Init(&hspi2, 1) != BMI088_NO_ERROR);
+    IMU_Param.scale[X] = 1;
+    IMU_Param.scale[Y] = 1;
+    IMU_Param.scale[Z] = 1;
+    IMU_Param.Yaw = 0;
+    IMU_Param.Pitch = 0;
+    IMU_Param.Roll = 0;
+    IMU_Param.flag = 1;
+
+    float init_quaternion[4] = {0};
+    InitQuaternion(init_quaternion);
+    IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
+    // imu heat init
+    PID_Init_Config_s config = {.MaxOut = 2000,
+                                .IntegralLimit = 300,
+                                .DeadBand = 0,
+                                .Kp = 1000,
+                                .Ki = 20,
+                                .Kd = 0,
+                                .Improve = 0x01}; // enable integratiaon limit
+    PIDInit(&TempCtrl, &config);
+
+    // noise of accel is relatively big and of high freq,thus lpf is used
+    INS.AccelLPF = 0.0085;
+    DWT_GetDeltaT(&INS_DWT_Count);
+    return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
+}
+
+/* 注意以1kHz的频率运行此任务 */
+void INS_Task(void)
+{
+    static uint32_t count = 0;
+    const float gravity[3] = {0, 0, 9.81f};
+
+    dt = DWT_GetDeltaT(&INS_DWT_Count);
+    t += dt;
+
+    // ins update
+    if ((count % 1) == 0)
+    {
+        BMI088_Read(&BMI088);
+
+        INS.Accel[X] = BMI088.Accel[X];
+        INS.Accel[Y] = BMI088.Accel[Y];
+        INS.Accel[Z] = BMI088.Accel[Z];
+        INS.Gyro[X] = BMI088.Gyro[X];
+        INS.Gyro[Y] = BMI088.Gyro[Y];
+        INS.Gyro[Z] = BMI088.Gyro[Z];
+
+        // demo function,用于修正安装误差,可以不管,本demo暂时没用
+        IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
+
+        // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
+        // INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
+        // INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
+
+        // 核心函数,EKF更新四元数
+        IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
+
+        memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
+
+        // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
+        BodyFrameToEarthFrame(xb, INS.xn, INS.q);
+        BodyFrameToEarthFrame(yb, INS.yn, INS.q);
+        BodyFrameToEarthFrame(zb, INS.zn, INS.q);
+
+        // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
+        float gravity_b[3];
+        EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
+        for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
+        {
+            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS
+                                   .AccelLPF / (INS.AccelLPF + dt);
+        }
+        BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
+
+        INS.Yaw = QEKF_INS.Yaw;
+        INS.Pitch = QEKF_INS.Pitch;
+        INS.Roll = QEKF_INS.Roll;
+        INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
+
+        // VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
+    }
+
+    // temperature control
+    if ((count % 2) == 0)
+    {
+        // 500hz
+        IMU_Temperature_Ctrl();
+    }
+
+    if ((count++ % 1000) == 0)
+    {
+        // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
+    }
+}
+
+/**
+ * @brief          Transform 3dvector from BodyFrame to EarthFrame
+ * @param[1]       vector in BodyFrame
+ * @param[2]       vector in EarthFrame
+ * @param[3]       quaternion
+ */
+void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
+{
+    vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
+                       (q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
+                       (q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
+
+    vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
+                       (0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
+                       (q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
+
+    vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
+                       (q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
+                       (0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
+}
+
+/**
+ * @brief          Transform 3dvector from EarthFrame to BodyFrame
+ * @param[1]       vector in EarthFrame
+ * @param[2]       vector in BodyFrame
+ * @param[3]       quaternion
+ */
+void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
+{
+    vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
+                       (q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
+                       (q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
+
+    vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
+                       (0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
+                       (q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
+
+    vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
+                       (q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
+                       (0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
+}
+
+/**
+ * @brief reserved.用于修正IMU安装误差与标度因数误差,即陀螺仪轴和云台轴的安装偏移
+ *
+ *
+ * @param param IMU参数
+ * @param gyro  角速度
+ * @param accel 加速度
+ */
+static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
+{
+    static float lastYawOffset, lastPitchOffset, lastRollOffset;
+    static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
+    float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
+
+    if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
+        fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
+        fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
+    {
+        cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
+        cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
+        cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
+        sinYaw = arm_sin_f32(param->Yaw / 57.295779513f);
+        sinPitch = arm_sin_f32(param->Pitch / 57.295779513f);
+        sinRoll = arm_sin_f32(param->Roll / 57.295779513f);
+
+        // 1.yaw(alpha) 2.pitch(beta) 3.roll(gamma)
+        c_11 = cosYaw * cosRoll + sinYaw * sinPitch * sinRoll;
+        c_12 = cosPitch * sinYaw;
+        c_13 = cosYaw * sinRoll - cosRoll * sinYaw * sinPitch;
+        c_21 = cosYaw * sinPitch * sinRoll - cosRoll * sinYaw;
+        c_22 = cosYaw * cosPitch;
+        c_23 = -sinYaw * sinRoll - cosYaw * cosRoll * sinPitch;
+        c_31 = -cosPitch * sinRoll;
+        c_32 = sinPitch;
+        c_33 = cosPitch * cosRoll;
+        param->flag = 0;
+    }
+    float gyro_temp[3];
+    for (uint8_t i = 0; i < 3; ++i)
+        gyro_temp[i] = gyro[i] * param->scale[i];
+
+    gyro[X] = c_11 * gyro_temp[X] +
+              c_12 * gyro_temp[Y] +
+              c_13 * gyro_temp[Z];
+    gyro[Y] = c_21 * gyro_temp[X] +
+              c_22 * gyro_temp[Y] +
+              c_23 * gyro_temp[Z];
+    gyro[Z] = c_31 * gyro_temp[X] +
+              c_32 * gyro_temp[Y] +
+              c_33 * gyro_temp[Z];
+
+    float accel_temp[3];
+    for (uint8_t i = 0; i < 3; ++i)
+        accel_temp[i] = accel[i];
+
+    accel[X] = c_11 * accel_temp[X] +
+               c_12 * accel_temp[Y] +
+               c_13 * accel_temp[Z];
+    accel[Y] = c_21 * accel_temp[X] +
+               c_22 * accel_temp[Y] +
+               c_23 * accel_temp[Z];
+    accel[Z] = c_31 * accel_temp[X] +
+               c_32 * accel_temp[Y] +
+               c_33 * accel_temp[Z];
+
+    lastYawOffset = param->Yaw;
+    lastPitchOffset = param->Pitch;
+    lastRollOffset = param->Roll;
+}
+
+//------------------------------------functions below are not used in this demo-------------------------------------------------
+//----------------------------------you can read them for learning or programming-----------------------------------------------
+//----------------------------------they could also be helpful for further design-----------------------------------------------
+
+/**
+ * @brief        Update quaternion
+ */
+void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
+{
+    float qa, qb, qc;
+
+    gx *= 0.5f * dt;
+    gy *= 0.5f * dt;
+    gz *= 0.5f * dt;
+    qa = q[0];
+    qb = q[1];
+    qc = q[2];
+    q[0] += (-qb * gx - qc * gy - q[3] * gz);
+    q[1] += (qa * gx + qc * gz - q[3] * gy);
+    q[2] += (qa * gy - qb * gz + q[3] * gx);
+    q[3] += (qa * gz + qb * gy - qc * gx);
+}
+
+/**
+ * @brief        Convert quaternion to eular angle
+ */
+void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
+{
+    *Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
+    *Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
+    *Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
+}
+
+/**
+ * @brief        Convert eular angle to quaternion
+ */
+void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
+{
+    float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
+    Yaw /= 57.295779513f;
+    Pitch /= 57.295779513f;
+    Roll /= 57.295779513f;
+    cosPitch = arm_cos_f32(Pitch / 2);
+    cosYaw = arm_cos_f32(Yaw / 2);
+    cosRoll = arm_cos_f32(Roll / 2);
+    sinPitch = arm_sin_f32(Pitch / 2);
+    sinYaw = arm_sin_f32(Yaw / 2);
+    sinRoll = arm_sin_f32(Roll / 2);
+    q[0] = cosPitch * cosRoll * cosYaw + sinPitch * sinRoll * sinYaw;
+    q[1] = sinPitch * cosRoll * cosYaw - cosPitch * sinRoll * sinYaw;
+    q[2] = sinPitch * cosRoll * sinYaw + cosPitch * sinRoll * cosYaw;
+    q[3] = cosPitch * cosRoll * sinYaw - sinPitch * sinRoll * cosYaw;
+}
diff --git a/User_Code/module/periph/imu/ins_task.h b/User_Code/module/periph/imu/ins_task.h
new file mode 100644
index 0000000..89c567c
--- /dev/null
+++ b/User_Code/module/periph/imu/ins_task.h
@@ -0,0 +1,143 @@
+/**
+ ******************************************************************************
+ * @file    ins_task.h
+ * @author  Wang Hongxi
+ * @author  annotation and modification by NeoZeng
+ * @version V2.0.0
+ * @date    2022/2/23
+ * @brief
+ ******************************************************************************
+ * @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
+ *            应用层调用初始化函数.
+ *
+ ******************************************************************************
+ */
+#ifndef __INS_TASK_H
+#define __INS_TASK_H
+
+#include "stdint.h"
+#include "BMI088driver.h"
+#include "QuaternionEKF.h"
+
+#define X 0
+#define Y 1
+#define Z 2
+
+#define INS_TASK_PERIOD 1
+
+typedef struct
+{
+    float Gyro[3]; // 角速度
+    float Accel[3]; // 加速度
+    // 还需要增加角速度数据
+    float Roll;
+    float Pitch;
+    float Yaw;
+    float YawTotalAngle;
+} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
+
+typedef struct
+{
+    float q[4]; // 四元数估计值
+
+    float MotionAccel_b[3]; // 机体坐标加速度
+    float MotionAccel_n[3]; // 绝对系加速度
+
+    float AccelLPF; // 加速度低通滤波系数
+
+    // bodyframe在绝对系的向量表示
+    float xn[3];
+    float yn[3];
+    float zn[3];
+
+    // 加速度在机体系和XY两轴的夹角
+    // float atanxz;
+    // float atanyz;
+
+    // IMU量测值
+    float Gyro[3]; // 角速度
+    float Accel[3]; // 加速度
+    // 位姿
+    float Roll;
+    float Pitch;
+    float Yaw;
+    float YawTotalAngle;
+
+    uint8_t init;
+} INS_t;
+
+/* 用于修正安装误差的参数 */
+typedef struct
+{
+    uint8_t flag;
+
+    float scale[3];
+
+    float Yaw;
+    float Pitch;
+    float Roll;
+} IMU_Param_t;
+
+/**
+ * @brief 初始化惯导解算系统
+ *
+ */
+attitude_t *INS_Init(void);
+
+/**
+ * @brief 此函数放入实时系统中,以1kHz频率运行
+ *        p.s. osDelay(1);
+ *
+ */
+void INS_Task(void);
+
+/**
+ * @brief 四元数更新函数,即实现dq/dt=0.5Ωq
+ *
+ * @param q  四元数
+ * @param gx
+ * @param gy
+ * @param gz
+ * @param dt 距离上次调用的时间间隔
+ */
+void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
+
+/**
+ * @brief 四元数转换成欧拉角 ZYX
+ *
+ * @param q
+ * @param Yaw
+ * @param Pitch
+ * @param Roll
+ */
+void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
+
+/**
+ * @brief ZYX欧拉角转换为四元数
+ *
+ * @param Yaw
+ * @param Pitch
+ * @param Roll
+ * @param q
+ */
+void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
+
+/**
+ * @brief 机体系到惯性系的变换函数
+ *
+ * @param vecBF body frame
+ * @param vecEF earth frame
+ * @param q
+ */
+void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
+
+/**
+ * @brief 惯性系转换到机体系
+ *
+ * @param vecEF
+ * @param vecBF
+ * @param q
+ */
+void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
+
+#endif
diff --git a/ozonedeb/windebnewestux.jdebug.user b/ozonedeb/windebnewestux.jdebug.user
index 7aebac3..44feebc 100644
--- a/ozonedeb/windebnewestux.jdebug.user
+++ b/ozonedeb/windebnewestux.jdebug.user
@@ -1,5 +1,5 @@
 
-OpenDocument="tasks.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=2297
+OpenDocument="tasks.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=2293
 OpenDocument="stm32h7xx_hal.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c", Line=329
 OpenDocument="list.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Middlewares/Third_Party/FreeRTOS/Source/list.c", Line=144
 OpenDocument="cmsis_gcc.h", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Drivers/CMSIS/Include/cmsis_gcc.h", Line=264
@@ -7,25 +7,25 @@ OpenDocument="cmsis_os2.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Midd
 OpenDocument="stm32h7xx_it.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Core/Src/stm32h7xx_it.c", Line=99
 OpenDocument="delayticks.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/User_Code/bsp/delayticks/delayticks.c", Line=0
 OpenDocument="buzzer.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/User_Code/module/periph/buzzer/buzzer.c", Line=191
-OpenDocument="main.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Core/Src/main.c", Line=69
+OpenDocument="main.c", FilePath="D:/RM/Elec Control/TronOneH7_Scaffold/Core/Src/main.c", Line=70
 OpenToolbar="Debug", Floating=0, x=0, y=0
 OpenToolbar="Breakpoints", Floating=0, x=1, y=0
 OpenWindow="Registers 1", DockArea=BOTTOM, x=1, y=0, w=362, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=551, h=273, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Disassembly", DockArea=BOTTOM, x=2, y=0, w=628, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Break & Tracepoints", DockArea=LEFT, x=0, y=1, w=551, h=290, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VectorCatchIndexMask=254
+OpenWindow="Break & Tracepoints", DockArea=LEFT, x=0, y=1, w=551, h=288, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VectorCatchIndexMask=254
 OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=466, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x20005E30
-OpenWindow="Global Data", DockArea=RIGHT, x=0, y=2, w=681, h=441, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=2, w=551, h=295, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Functions", DockArea=LEFT, x=0, y=3, w=551, h=214, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Call Graph", DockArea=LEFT, x=0, y=3, w=551, h=214, TabPos=2, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Global Data", DockArea=RIGHT, x=0, y=2, w=681, h=437, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=2, w=551, h=294, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Functions", DockArea=LEFT, x=0, y=3, w=551, h=217, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Call Graph", DockArea=LEFT, x=0, y=3, w=551, h=217, TabPos=2, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=1101, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=681, h=423, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="491;-46", PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="459;-65", CodeGraphLegendShown=1, CodeGraphLegendPosition="475;0"
-OpenWindow="Console", DockArea=LEFT, x=0, y=3, w=551, h=214, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="RTOS", DockArea=RIGHT, x=0, y=0, w=681, h=229, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing=""
+OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=681, h=420, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="491;-46", PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="459;-65", CodeGraphLegendShown=1, CodeGraphLegendPosition="475;0"
+OpenWindow="Console", DockArea=LEFT, x=0, y=3, w=551, h=217, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="RTOS", DockArea=RIGHT, x=0, y=0, w=681, h=236, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing=""
 TableHeader="Call Graph", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Stack Total";"Stack Local";"Code Total";"Code Local";"Depth";"Called From"], ColWidths=[384;100;100;100;100;100;118]
 TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1164;100;100;100;100]
-TableHeader="Global Data", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Location";"Size";"Type";"Scope"], ColWidths=[222;130;100;54;95;100]
+TableHeader="Global Data", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Location";"Size";"Type";"Scope"], ColWidths=[222;130;100;54;95;414]
 TableHeader="Vector Catches", SortCol="None", SortOrder="ASCENDING", VisibleCols=["";"Vector Catch";"Description"], ColWidths=[50;300;500]
 TableHeader="Break & Tracepoints", SortCol="None", SortOrder="ASCENDING", VisibleCols=["";"Type";"Location";"Extras"], ColWidths=[100;100;142;209]
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[229;100;100;100;902]