imu test

2026-07-23 19:25:09 +08:00 · 2025-11-16 17:19:14 +08:00
parent de8a0d5117
commit ab01747052
20 changed files with 10796 additions and 234 deletions
--- a/.mxproject
+++ b/.mxproject
--- a/Core/Src/freertos.c
+++ b/Core/Src/freertos.c
@@ -26,7 +26,7 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "ws2812status.h"
-
+#include "ins_task.h"
 #include "buzzer.h"
 // #include "remoteTask.h"
 //
@@ -58,65 +58,65 @@
 /* Definitions for BeginTask */
 osThreadId_t BeginTaskHandle;
 const osThreadAttr_t BeginTask_attributes = {
-  .name = "BeginTask",
-  .stack_size = 256 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "BeginTask",
+        .stack_size = 256 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for shoot */
 osThreadId_t shootHandle;
 const osThreadAttr_t shoot_attributes = {
-  .name = "shoot",
-  .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "shoot",
+        .stack_size = 512 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for gimbal */
 osThreadId_t gimbalHandle;
 const osThreadAttr_t gimbal_attributes = {
-  .name = "gimbal",
-  .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "gimbal",
+        .stack_size = 512 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for chassis */
 osThreadId_t chassisHandle;
 const osThreadAttr_t chassis_attributes = {
-  .name = "chassis",
-  .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "chassis",
+        .stack_size = 512 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for init */
 osThreadId_t initHandle;
 const osThreadAttr_t init_attributes = {
-  .name = "init",
-  .stack_size = 256 * 4,
-  .priority = (osPriority_t) osPriorityHigh,
+        .name = "init",
+        .stack_size = 256 * 4,
+        .priority = (osPriority_t) osPriorityHigh,
 };
 /* Definitions for vision */
 osThreadId_t visionHandle;
 const osThreadAttr_t vision_attributes = {
-  .name = "vision",
-  .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "vision",
+        .stack_size = 512 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for cmd */
 osThreadId_t cmdHandle;
 const osThreadAttr_t cmd_attributes = {
-  .name = "cmd",
-  .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "cmd",
+        .stack_size = 512 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for reference */
 osThreadId_t referenceHandle;
 const osThreadAttr_t reference_attributes = {
-  .name = "reference",
-  .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+        .name = "reference",
+        .stack_size = 512 * 4,
+        .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for WS2812Task */
 osThreadId_t WS2812TaskHandle;
 const osThreadAttr_t WS2812Task_attributes = {
-  .name = "WS2812Task",
-  .stack_size = 256 * 4,
-  .priority = (osPriority_t) osPriorityBelowNormal,
+        .name = "WS2812Task",
+        .stack_size = 256 * 4,
+        .priority = (osPriority_t) osPriorityBelowNormal,
 };

 /* Private function prototypes -----------------------------------------------*/
@@ -125,43 +125,53 @@ const osThreadAttr_t WS2812Task_attributes = {
 /* USER CODE END FunctionPrototypes */

 void StartDefaultTask(void *argument);
+
 void ShootTask(void *argument);
+
 void GimbalTask(void *argument);
+
 void ChassisTask(void *argument);
+
 void StartInitTask(void *argument);
+
 void VisionTask(void *argument);
+
 void CmdTask(void *argument);
+
 void RefereeTask(void *argument);
-extern void ws2812Task(void *argument);

 extern void MX_USB_DEVICE_Init(void);
+
 void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

 /* Hook prototypes */
 void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName);
+
 void vApplicationMallocFailedHook(void);

 /* USER CODE BEGIN 4 */
-void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName) {
-  /* Run time stack overflow checking is performed if
-  configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
-  called if a stack overflow is detected. */
+void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName)
+{
+    /* Run time stack overflow checking is performed if
+    configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
+    called if a stack overflow is detected. */
 }

 /* USER CODE END 4 */

 /* USER CODE BEGIN 5 */
-void vApplicationMallocFailedHook(void) {
-  /* vApplicationMallocFailedHook() will only be called if
-  configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h. It is a hook
-  function that will get called if a call to pvPortMalloc() fails.
-  pvPortMalloc() is called internally by the kernel whenever a task, queue,
-  timer or semaphore is created. It is also called by various parts of the
-  demo application. If heap_1.c or heap_2.c are used, then the size of the
-  heap available to pvPortMalloc() is defined by configTOTAL_HEAP_SIZE in
-  FreeRTOSConfig.h, and the xPortGetFreeHeapSize() API function can be used
-  to query the size of free heap space that remains (although it does not
-  provide information on how the remaining heap might be fragmented). */
+void vApplicationMallocFailedHook(void)
+{
+    /* vApplicationMallocFailedHook() will only be called if
+    configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h. It is a hook
+    function that will get called if a call to pvPortMalloc() fails.
+    pvPortMalloc() is called internally by the kernel whenever a task, queue,
+    timer or semaphore is created. It is also called by various parts of the
+    demo application. If heap_1.c or heap_2.c are used, then the size of the
+    heap available to pvPortMalloc() is defined by configTOTAL_HEAP_SIZE in
+    FreeRTOSConfig.h, and the xPortGetFreeHeapSize() API function can be used
+    to query the size of free heap space that remains (although it does not
+    provide information on how the remaining heap might be fragmented). */
 }

 /* USER CODE END 5 */
@@ -171,62 +181,63 @@ void vApplicationMallocFailedHook(void) {
  * @param  None
  * @retval None
  */
-void MX_FREERTOS_Init(void) {
-  /* USER CODE BEGIN Init */
+void MX_FREERTOS_Init(void)
+{
+    /* USER CODE BEGIN Init */

-  /* USER CODE END Init */
+    /* USER CODE END Init */

-  /* USER CODE BEGIN RTOS_MUTEX */
-  /* add mutexes, ... */
-  /* USER CODE END RTOS_MUTEX */
+    /* USER CODE BEGIN RTOS_MUTEX */
+    /* add mutexes, ... */
+    /* USER CODE END RTOS_MUTEX */

-  /* USER CODE BEGIN RTOS_SEMAPHORES */
-  /* add semaphores, ... */
-  /* USER CODE END RTOS_SEMAPHORES */
+    /* USER CODE BEGIN RTOS_SEMAPHORES */
+    /* add semaphores, ... */
+    /* USER CODE END RTOS_SEMAPHORES */

-  /* USER CODE BEGIN RTOS_TIMERS */
-  /* start timers, add new ones, ... */
-  /* USER CODE END RTOS_TIMERS */
+    /* USER CODE BEGIN RTOS_TIMERS */
+    /* start timers, add new ones, ... */
+    /* USER CODE END RTOS_TIMERS */

-  /* USER CODE BEGIN RTOS_QUEUES */
-  /* add queues, ... */
-  /* USER CODE END RTOS_QUEUES */
+    /* USER CODE BEGIN RTOS_QUEUES */
+    /* add queues, ... */
+    /* USER CODE END RTOS_QUEUES */

-  /* Create the thread(s) */
-  /* creation of BeginTask */
-  BeginTaskHandle = osThreadNew(StartDefaultTask, NULL, &BeginTask_attributes);
+    /* Create the thread(s) */
+    /* creation of BeginTask */
+    BeginTaskHandle = osThreadNew(StartDefaultTask, NULL, &BeginTask_attributes);

-  /* creation of shoot */
-  shootHandle = osThreadNew(ShootTask, NULL, &shoot_attributes);
+    /* creation of shoot */
+    shootHandle = osThreadNew(ShootTask, NULL, &shoot_attributes);

-  /* creation of gimbal */
-  gimbalHandle = osThreadNew(GimbalTask, NULL, &gimbal_attributes);
+    /* creation of gimbal */
+    gimbalHandle = osThreadNew(GimbalTask, NULL, &gimbal_attributes);

-  /* creation of chassis */
-  chassisHandle = osThreadNew(ChassisTask, NULL, &chassis_attributes);
+    /* creation of chassis */
+    chassisHandle = osThreadNew(ChassisTask, NULL, &chassis_attributes);

-  /* creation of init */
-  initHandle = osThreadNew(StartInitTask, NULL, &init_attributes);
+    /* creation of init */
+    initHandle = osThreadNew(StartInitTask, NULL, &init_attributes);

-  /* creation of vision */
-  visionHandle = osThreadNew(VisionTask, NULL, &vision_attributes);
+    /* creation of vision */
+    visionHandle = osThreadNew(VisionTask, NULL, &vision_attributes);

-  /* creation of cmd */
-  cmdHandle = osThreadNew(CmdTask, NULL, &cmd_attributes);
+    /* creation of cmd */
+    cmdHandle = osThreadNew(CmdTask, NULL, &cmd_attributes);

-  /* creation of reference */
-  referenceHandle = osThreadNew(RefereeTask, NULL, &reference_attributes);
+    /* creation of reference */
+    referenceHandle = osThreadNew(RefereeTask, NULL, &reference_attributes);

-  /* creation of WS2812Task */
-  WS2812TaskHandle = osThreadNew(ws2812Task, NULL, &WS2812Task_attributes);
+    /* creation of WS2812Task */
+    WS2812TaskHandle = osThreadNew(ws2812Task, NULL, &WS2812Task_attributes);

-  /* USER CODE BEGIN RTOS_THREADS */
-  /* add threads, ... */
-  /* USER CODE END RTOS_THREADS */
+    /* USER CODE BEGIN RTOS_THREADS */
+    /* add threads, ... */
+    /* USER CODE END RTOS_THREADS */

-  /* USER CODE BEGIN RTOS_EVENTS */
-  /* add events, ... */
-  /* USER CODE END RTOS_EVENTS */
+    /* USER CODE BEGIN RTOS_EVENTS */
+    /* add events, ... */
+    /* USER CODE END RTOS_EVENTS */

 }

@@ -239,14 +250,15 @@ void MX_FREERTOS_Init(void) {
 /* USER CODE END Header_StartDefaultTask */
 __weak void StartDefaultTask(void *argument)
 {
-  /* init code for USB_DEVICE */
-  MX_USB_DEVICE_Init();
-  /* USER CODE BEGIN StartDefaultTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END StartDefaultTask */
+    /* init code for USB_DEVICE */
+    MX_USB_DEVICE_Init();
+    /* USER CODE BEGIN StartDefaultTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END StartDefaultTask */
 }

 /* USER CODE BEGIN Header_ShootTask */
@@ -258,12 +270,13 @@ __weak void StartDefaultTask(void *argument)
 /* USER CODE END Header_ShootTask */
 __weak void ShootTask(void *argument)
 {
-  /* USER CODE BEGIN ShootTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END ShootTask */
+    /* USER CODE BEGIN ShootTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END ShootTask */
 }

 /* USER CODE BEGIN Header_GimbalTask */
@@ -275,12 +288,13 @@ __weak void ShootTask(void *argument)
 /* USER CODE END Header_GimbalTask */
 __weak void GimbalTask(void *argument)
 {
-  /* USER CODE BEGIN GimbalTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END GimbalTask */
+    /* USER CODE BEGIN GimbalTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END GimbalTask */
 }

 /* USER CODE BEGIN Header_ChassisTask */
@@ -292,12 +306,13 @@ __weak void GimbalTask(void *argument)
 /* USER CODE END Header_ChassisTask */
 __weak void ChassisTask(void *argument)
 {
-  /* USER CODE BEGIN ChassisTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END ChassisTask */
+    /* USER CODE BEGIN ChassisTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END ChassisTask */
 }

 /* USER CODE BEGIN Header_StartInitTask */
@@ -309,12 +324,13 @@ __weak void ChassisTask(void *argument)
 /* USER CODE END Header_StartInitTask */
 __weak void StartInitTask(void *argument)
 {
-  /* USER CODE BEGIN StartInitTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END StartInitTask */
+    /* USER CODE BEGIN StartInitTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END StartInitTask */
 }

 /* USER CODE BEGIN Header_VisionTask */
@@ -326,12 +342,13 @@ __weak void StartInitTask(void *argument)
 /* USER CODE END Header_VisionTask */
 __weak void VisionTask(void *argument)
 {
-  /* USER CODE BEGIN VisionTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END VisionTask */
+    /* USER CODE BEGIN VisionTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END VisionTask */
 }

 /* USER CODE BEGIN Header_CmdTask */
@@ -343,12 +360,13 @@ __weak void VisionTask(void *argument)
 /* USER CODE END Header_CmdTask */
 __weak void CmdTask(void *argument)
 {
-  /* USER CODE BEGIN CmdTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END CmdTask */
+    /* USER CODE BEGIN CmdTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END CmdTask */
 }

 /* USER CODE BEGIN Header_RefereeTask */
@@ -360,16 +378,37 @@ __weak void CmdTask(void *argument)
 /* USER CODE END Header_RefereeTask */
 __weak void RefereeTask(void *argument)
 {
-  /* USER CODE BEGIN RefereeTask */
-  /* Infinite loop */
-  for (;;) {
-    osDelay(1);
-  }
-  /* USER CODE END RefereeTask */
+    /* USER CODE BEGIN RefereeTask */
+    /* Infinite loop */
+    for (;;)
+    {
+        osDelay(1);
+    }
+    /* USER CODE END RefereeTask */
 }

 /* Private application code --------------------------------------------------*/
 /* USER CODE BEGIN Application */

+//Todo:INS_Task是测试阶段使用。
+__attribute__((noreturn)) void StartINSTASK(void const *argument)
+{
+    static float ins_start;
+    static float ins_dt;
+    INS_Init(); // 确保BMI088被正确初始化.
+    // LOGINFO("[freeRTOS] INS Task Start");
+    for (;;)
+    {
+        // 1kHz
+        // ins_start = DWT_GetTimeline_ms();
+        INS_Task();
+        // ins_dt = DWT_GetTimeline_ms() - ins_start;
+        // if (ins_dt > 1)
+        //     Todo: LOGERROR("[freeRTOS] INS Task is being DELAY! dt = [%f]", &ins_dt);
+        // // VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织
+        osDelay(1); // 1ms, 1kHz
+    }
+}
+
 /* USER CODE END Application */

--- a/Middlewares/ST/ARM/DSP/Inc/arm_math.h
+++ b/Middlewares/ST/ARM/DSP/Inc/arm_math.h
--- a/Middlewares/Third_Party/ARM/DSP/LICENSE.txt
+++ b/Middlewares/Third_Party/ARM/DSP/LICENSE.txt
@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "{}"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright {yyyy} {name of copyright owner}
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
--- a/TronOneH7_Scaffold.ioc
+++ b/TronOneH7_Scaffold.ioc
@@ -498,12 +498,14 @@ Mcu.Pin55=VP_TIM1_VS_ClockSourceINT
 Mcu.Pin56=VP_TIM1_VS_no_output3
 Mcu.Pin57=VP_USB_DEVICE_VS_USB_DEVICE_CDC_HS
 Mcu.Pin58=VP_MEMORYMAP_VS_MEMORYMAP
+Mcu.Pin59=VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0
 Mcu.Pin6=PC0
 Mcu.Pin7=PC1
 Mcu.Pin8=PC2_C
 Mcu.Pin9=PC3_C
-Mcu.PinsNb=59
-Mcu.ThirdPartyNb=0
+Mcu.PinsNb=60
+Mcu.ThirdParty0=STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0
+Mcu.ThirdPartyNb=1
 Mcu.UserConstants=
 Mcu.UserName=STM32H723VGTx
 MxCube.Version=6.15.0
@@ -874,6 +876,10 @@ SPI6.Direction=SPI_DIRECTION_2LINES_TXONLY
 SPI6.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPhase
 SPI6.Mode=SPI_MODE_MASTER
 SPI6.VirtualType=VM_MASTER
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.DSPOoLibraryJjLibrary_Checked=true
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.IPParameters=LibraryCcDSPOoLibraryJjDSPOoLibrary
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.LibraryCcDSPOoLibraryJjDSPOoLibrary=true
+STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0_SwParameter=LibraryCcDSPOoLibraryJjDSPOoLibrary\:true;
 TIM1.Channel-PWM\ Generation3\ No\ Output=TIM_CHANNEL_3
 TIM1.IPParameters=Channel-PWM Generation3 No Output,Prescaler,Period
 TIM1.Period=1999
@@ -929,6 +935,8 @@ VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
 VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg
 VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP
+VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0.Mode=DSPOoLibraryJjLibrary
+VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0.Signal=STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0
 VP_SYS_VS_tim23.Mode=TIM23
 VP_SYS_VS_tim23.Signal=SYS_VS_tim23
 VP_TIM1_VS_ClockSourceINT.Mode=Internal
--- a/User_Code/application/robot.c
+++ b/User_Code/application/robot.c
@@ -0,0 +1,86 @@
+/**
+* @file robot.c
+ * @brief
+ * @author TuxMonkey (nqx2004@gmail.com)
+ * @version 1.0
+ * @date 2025-07-08
+ *
+ * @copyright Copyright (c) 2025  DLMU-C.ONE
+ *
+ * @par 修改日志:
+ * <table>
+ * <tr><th>Date       <th>Version <th>Author  <th>Description
+ * <tr><td>2025-07-08 <td>1.0     <td>TuxMonkey     <td>内容
+ * </table>
+ */
+
+#include "bsp_init.h"
+#include "robot.h"
+
+#include "cmsis_gcc.h"
+// #include "robot_def.h"
+// #include "robot_task.h"
+
+// // 编译warning,提醒开发者修改机器人参数
+// #ifndef ROBOT_DEF_PARAM_WARNING
+// #define ROBOT_DEF_PARAM_WARNING
+// #pragma message "check if you have configured the parameters in robot_def.h, IF NOT, please refer to the comments AND DO IT, otherwise the robot will have FATAL ERRORS!!!"
+// #endif // !ROBOT_DEF_PARAM_WARNING
+
+// #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
+// #include "chassis.h"
+// #endif
+
+// #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
+// #include "gimbal.h"
+// #include "shoot.h"
+// #include "robot_cmd.h"
+// #endif
+
+/**
+ * @brief 机器人初始化函数
+ *
+ * @note 该函数在系统启动时调用,用于初始化机器人各个模块
+ *
+ * @attention 请不要在初始化过程中使用中断和延时函数！
+ * 若必须,则只允许使用DWT_Delay()
+ */
+
+void RobotInit()
+{
+    // 关闭中断,防止在初始化过程中发生中断
+    // 请不要在初始化过程中使用中断和延时函数！
+    // 若必须,则只允许使用DWT_Delay()
+    __disable_irq();
+
+    BSPInit();
+
+#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
+    RobotCMDInit();
+    // GimbalInit();
+    // ShootInit();
+#endif
+
+#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
+    ChassisInit();
+#endif
+
+    // OSTaskInit(); // 创建基础任务
+
+    // 初始化完成,开启中断
+    __enable_irq();
+}
+
+// void RobotTask()
+// {
+// #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
+//     RobotCMDTask();
+//     // GimbalTask();
+//     // ShootTask();
+// #endif
+
+// #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
+//     ChassisTask();
+// #endif
+
+// }
--- a/User_Code/application/robot.h
+++ b/User_Code/application/robot.h
@@ -0,0 +1,18 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
+
+/**
+ * @brief 机器人初始化,请在开启rtos之前调用.这也是唯一需要放入main函数的函数
+ *
+ */
+void RobotInit();
+
+/**
+ * @brief 机器人任务,放入实时系统以一定频率运行,内部会调用各个应用的任务
+ *
+ */
+void RobotTask();
+
+#endif
--- a/User_Code/bsp/bsp_init.h
+++ b/User_Code/bsp/bsp_init.h
@@ -0,0 +1,22 @@
+#ifndef BSP_INIT_h
+#define BSP_INIT_h
+
+#include "bsp_init.h"
+// #include "bsp_log.h"
+#include "bsp_dwt.h"
+// #include "bsp_usb.h"
+
+/**
+ * @brief bsp层初始化统一入口,这里仅初始化必须的bsp组件,其他组件的初始化在各自的模块中进行
+ *        需在实时系统启动前调用,目前由RobotoInit()调用
+ * 
+ * @note 其他实例型的外设如CAN和串口会在注册实例的时候自动初始化,不注册不初始化
+*/
+inline void BSPInit()
+{
+    DWT_Init(480); //Todo:修改这里以适配喵板cpufreq
+    // BSPLogInit();
+}
+
+#endif // !BSP_INIT_h
+
--- a/User_Code/bsp/dwt/bsp_dwt.c
+++ b/User_Code/bsp/dwt/bsp_dwt.c
@@ -1,5 +1,139 @@
-//
-// Created by tuxmonkey on 2025/10/28.
-//
+/**
+ * @file bsp_dwt.c
+ * @brief DWT延时 软件延时库
+ * @author TuxMonkey (nqx2004@gmail.com)
+ * @version 1.0
+ * @date 2025-07-08
+ *
+ * @copyright Copyright (c) 2025  DLMU-C.ONE
+ *
+ * @par 修改日志:
+ * <table>
+ * <tr><th>Date       <th>Version <th>Author  <th>Description
+ * <tr><td>2025-07-08 <td>1.0     <td>TuxMonkey     <td>内容
+ * </table>
+ */

-#include "bsp_dwt.h"
+#include "bsp_dwt.h"
+#include "cmsis_os.h"
+
+static DWT_Time_t SysTime;
+static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
+static uint32_t CYCCNT_RountCount;
+static uint32_t CYCCNT_LAST;
+static uint64_t CYCCNT64;
+
+/**
+ * @brief 私有函数,用于检查DWT CYCCNT寄存器是否溢出,并更新CYCCNT_RountCount
+ * @attention 此函数假设两次调用之间的时间间隔不超过一次溢出
+ *
+ * @todo 更好的方案是为dwt的时间更新单独设置一个任务?
+ *       不过,使用dwt的初衷是定时不被中断/任务等因素影响,因此该实现仍然有其存在的意义
+ *
+ */
+static void DWT_CNT_Update(void)
+{
+    static volatile uint8_t bit_locker = 0;
+    if (!bit_locker)
+    {
+        bit_locker = 1;
+        volatile uint32_t cnt_now = DWT->CYCCNT;
+        if (cnt_now < CYCCNT_LAST)
+            CYCCNT_RountCount++;
+
+        CYCCNT_LAST = DWT->CYCCNT;
+        bit_locker = 0;
+    }
+}
+
+void DWT_Init(uint32_t CPU_Freq_mHz)
+{
+    /* 使能DWT外设 */
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+
+    /* DWT CYCCNT寄存器计数清0 */
+    DWT->CYCCNT = (uint32_t) 0u;
+
+    /* 使能Cortex-M DWT CYCCNT寄存器 */
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+
+    CPU_FREQ_Hz = CPU_Freq_mHz * 1000000;
+    CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000;
+    CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000;
+    CYCCNT_RountCount = 0;
+
+    DWT_CNT_Update();
+}
+
+float DWT_GetDeltaT(uint32_t *cnt_last)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    float dt = ((uint32_t) (cnt_now - *cnt_last)) / ((float) (CPU_FREQ_Hz));
+    *cnt_last = cnt_now;
+
+    DWT_CNT_Update();
+
+    return dt;
+}
+
+double DWT_GetDeltaT64(uint32_t *cnt_last)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    double dt = ((uint32_t) (cnt_now - *cnt_last)) / ((double) (CPU_FREQ_Hz));
+    *cnt_last = cnt_now;
+
+    DWT_CNT_Update();
+
+    return dt;
+}
+
+void DWT_SysTimeUpdate(void)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3;
+
+    DWT_CNT_Update();
+
+    CYCCNT64 = (uint64_t) CYCCNT_RountCount * (uint64_t) UINT32_MAX + (uint64_t) cnt_now;
+    CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz;
+    CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz;
+    SysTime.s = CNT_TEMP1;
+    SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms;
+    CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms;
+    SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us;
+}
+
+float DWT_GetTimeline_s(void)
+{
+    DWT_SysTimeUpdate();
+
+    float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f;
+
+    return DWT_Timelinef32;
+}
+
+float DWT_GetTimeline_ms(void)
+{
+    DWT_SysTimeUpdate();
+
+    float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f;
+
+    return DWT_Timelinef32;
+}
+
+uint64_t DWT_GetTimeline_us(void)
+{
+    DWT_SysTimeUpdate();
+
+    uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us;
+
+    return DWT_Timelinef32;
+}
+
+void DWT_Delay(float Delay)
+{
+    uint32_t tickstart = DWT->CYCCNT;
+    float wait = Delay;
+
+    while ((DWT->CYCCNT - tickstart) < wait * (float) CPU_FREQ_Hz);
+}
--- a/User_Code/bsp/dwt/bsp_dwt.h
+++ b/User_Code/bsp/dwt/bsp_dwt.h
@@ -1,8 +1,104 @@
-//
-// Created by tuxmonkey on 2025/10/28.
-//
+/**
+ * @file bsp_dwt.h
+ * @brief DWT延时 软件延时库头文件
+ * @author TuxMonkey (nqx2004@gmail.com)
+ * @version 1.0
+ * @date 2025-07-08
+ *
+ * @copyright Copyright (c) 2025  DLMU-C.ONE
+ *
+ * @par 修改日志:
+ * <table>
+ * <tr><th>Date       <th>Version <th>Author  <th>Description
+ * <tr><td>2025-07-08 <td>1.0     <td>TuxMonkey     <td>内容
+ * </table>
+ */

-#ifndef TRONONEH7_SCAFFOLD_BSP_DWT_H
-#define TRONONEH7_SCAFFOLD_BSP_DWT_H
+#ifndef _BSP_DWT_H
+#define _BSP_DWT_H

-#endif //TRONONEH7_SCAFFOLD_BSP_DWT_H
+#include "main.h"
+#include "stdint.h"
+#include "bsp_log.h"
+
+typedef struct
+{
+    uint32_t s;
+    uint16_t ms;
+    uint16_t us;
+} DWT_Time_t;
+
+/**
+ * @brief 该宏用于计算代码段执行时间,单位为秒/s,返回值为float类型
+ *        首先需要创建一个float类型的变量,用于存储时间间隔
+ *        计算得到的时间间隔同时还会通过RTT打印到日志终端,你也可以将你的dt变量添加到查看
+ */
+#define TIME_ELAPSE(dt, code)                    \
+    do                                           \
+    {                                            \
+        float tstart = DWT_GetTimeline_s();      \
+        code;                                    \
+        dt = DWT_GetTimeline_s() - tstart;       \
+        LOGINFO("[DWT] " #dt " = %f s\r\n", dt); \
+    } while (0)
+
+/**
+ * @brief 初始化DWT,传入参数为CPU频率,单位MHz
+ *
+ * @param CPU_Freq_mHz c板为168MHz,A板为180MHz
+ */
+void DWT_Init(uint32_t CPU_Freq_mHz);
+
+/**
+ * @brief 获取两次调用之间的时间间隔,单位为秒/s
+ *
+ * @param cnt_last 上一次调用的时间戳
+ * @return float 时间间隔,单位为秒/s
+ */
+float DWT_GetDeltaT(uint32_t *cnt_last);
+
+/**
+ * @brief 获取两次调用之间的时间间隔,单位为秒/s,高精度
+ *
+ * @param cnt_last 上一次调用的时间戳
+ * @return double 时间间隔,单位为秒/s
+ */
+double DWT_GetDeltaT64(uint32_t *cnt_last);
+
+/**
+ * @brief 获取当前时间,单位为秒/s,即初始化后的时间
+ *
+ * @return float 时间轴
+ */
+float DWT_GetTimeline_s(void);
+
+/**
+ * @brief 获取当前时间,单位为毫秒/ms,即初始化后的时间
+ *
+ * @return float
+ */
+float DWT_GetTimeline_ms(void);
+
+/**
+ * @brief 获取当前时间,单位为微秒/us,即初始化后的时间
+ *
+ * @return uint64_t
+ */
+uint64_t DWT_GetTimeline_us(void);
+
+/**
+ * @brief DWT延时函数,单位为秒/s
+ * @attention 该函数不受中断是否开启的影响,可以在临界区和关闭中断时使用
+ * @note 禁止在__disable_irq()和__enable_irq()之间使用HAL_Delay()函数,应使用本函数
+ *
+ * @param Delay 延时时间,单位为秒/s
+ */
+void DWT_Delay(float Delay);
+
+/**
+ * @brief DWT更新时间轴函数,会被三个timeline函数调用
+ * @attention 如果长时间不调用timeline函数,则需要手动调用该函数更新时间轴,否则CYCCNT溢出后定时和时间轴不准确
+ */
+void DWT_SysTimeUpdate(void);
+
+#endif /* BSP_DWT_H_ */
--- a/User_Code/bsp/dwt/bsp_dwt.md
+++ b/User_Code/bsp/dwt/bsp_dwt.md
@@ -0,0 +1,54 @@
+# bsp_dwt
+
+DWT是stm32内部的一个"隐藏资源",他的用途是给下载器提供准确的定时,从而为调试信息加上时间戳.并在固定的时间间隔将调试数据发送到你的xxlink上.
+
+## 常用功能
+
+### 计算两次进入同一个函数的时间间隔
+
+```c
+static uint32_t cnt;
+float deltaT;
+
+deltaT=DWT_GetDeltaT(&cnt);
+```
+
+### 计算执行某部分代码的耗时
+
+```c
+float start,end;
+start=DWT_DetTimeline_ms();
+
+// some proc to go... 
+for(uint8_t i=0;i<10;i++)
+ foo();
+
+end = DWT_DetTimeline_ms()-start;
+```
+
+我们还提供了一个宏用于调试计时:
+
+```c
+#define TIME_ELAPSE(dt, code)                    \
+    do                                           \
+    {                                            \
+        float tstart = DWT_GetTimeline_s();      \
+        code;                                    \
+        dt = DWT_GetTimeline_s() - tstart;       \
+        LOGINFO("[DWT] " #dt " = %f s\r\n", dt); \
+    } while (0)
+
+```
+
+传入一个float类型的变量,并将你要执行的代码写入第二个参数:
+
+```c
+    static float my_func_dt;
+    TIME_ELAPSE(my_func_dt,
+                Function1(vara);
+                Function2(some, var);
+                Function3(your,param);
+                 // something more
+                 );
+    // my_func_dt can be used for other purpose then;
+```
--- a/User_Code/bsp/fdcan/bsp_fdcan.c
+++ b/User_Code/bsp/fdcan/bsp_fdcan.c
@@ -150,7 +150,7 @@ FDCANInstance *FDCANRegister(FDCAN_Init_Config_s *config)
    {
        while (1)
        {
-            // LOGERROR("[bsp_fdcan] FDCAN instance exceeded MAX num, consider balance the load of FDCAN bus");
+            // Todo:LOGERROR("[bsp_fdcan] FDCAN instance exceeded MAX num, consider balance the load of FDCAN bus");
        }
    }
    for (size_t i = 0; i < idx; i++)
@@ -160,7 +160,7 @@ FDCANInstance *FDCANRegister(FDCAN_Init_Config_s *config)
        {
            while (1)
            {
-                // LOGERROR("[bsp_fdcan] FDCAN id crash ,tx [%d] or rx [%d] already registered", config->tx_id, config->rx_id);
+                // Todo:LOGERROR("[bsp_fdcan] FDCAN id crash ,tx [%d] or rx [%d] already registered", config->tx_id, config->rx_id);
            }
        }
    }
@@ -243,7 +243,7 @@ void FDCANSetDLC(FDCANInstance *_instance, uint8_t length)
    if (length > 64 || length == 0) // 安全检查
        while (1)
        {
-            //LOGERROR("[bsp_fdcan] FDCAN DLC error! check your code or wild pointer");
+            //Todo:LOGERROR("[bsp_fdcan] FDCAN DLC error! check your code or wild pointer");
        }
    _instance->txconf.DataLength = FDCANLenToDlc(length);
 }
--- a/User_Code/module/algorithm/controller/controller.c
+++ b/User_Code/module/algorithm/controller/controller.c
@@ -1,5 +1,208 @@
-//
-// Created by ASUS on 2025/11/15.
-//
-
+/**
+ * @file controller.c
+ * @author wanghongxi
+ * @author modified by neozng
+ * @brief  PID控制器定义
+ * @version beta
+ * @date 2022-11-01
+ *
+ * @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
+ */
 #include "controller.h"
+#include "memory.h"
+
+/* ----------------------------下面是pid优化环节的实现---------------------------- */
+
+// 梯形积分
+static void f_Trapezoid_Intergral(PIDInstance *pid)
+{
+    // 计算梯形的面积,(上底+下底)*高/2
+    pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
+}
+
+// 变速积分(误差小时积分作用更强)
+static void f_Changing_Integration_Rate(PIDInstance *pid)
+{
+    if (pid->Err * pid->Iout > 0)
+    {
+        // 积分呈累积趋势
+        if (abs(pid->Err) <= pid->CoefB)
+            return; // Full integral
+        if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
+            pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
+        else // 最大阈值,不使用积分
+            pid->ITerm = 0;
+    }
+}
+
+static void f_Integral_Limit(PIDInstance *pid)
+{
+    static float temp_Output, temp_Iout;
+    temp_Iout = pid->Iout + pid->ITerm;
+    temp_Output = pid->Pout + pid->Iout + pid->Dout;
+    if (abs(temp_Output) > pid->MaxOut)
+    {
+        if (pid->Err * pid->Iout > 0) // 积分却还在累积
+        {
+            pid->ITerm = 0; // 当前积分项置零
+        }
+    }
+
+    if (temp_Iout > pid->IntegralLimit)
+    {
+        pid->ITerm = 0;
+        pid->Iout = pid->IntegralLimit;
+    }
+    if (temp_Iout < -pid->IntegralLimit)
+    {
+        pid->ITerm = 0;
+        pid->Iout = -pid->IntegralLimit;
+    }
+}
+
+// 微分先行(仅使用反馈值而不计参考输入的微分)
+static void f_Derivative_On_Measurement(PIDInstance *pid)
+{
+    pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
+}
+
+// 微分滤波(采集微分时,滤除高频噪声)
+static void f_Derivative_Filter(PIDInstance *pid)
+{
+    pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
+                pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
+}
+
+// 输出滤波
+static void f_Output_Filter(PIDInstance *pid)
+{
+    pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
+                  pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
+}
+
+// 输出限幅
+static void f_Output_Limit(PIDInstance *pid)
+{
+    if (pid->Output > pid->MaxOut)
+    {
+        pid->Output = pid->MaxOut;
+    }
+    if (pid->Output < -(pid->MaxOut))
+    {
+        pid->Output = -(pid->MaxOut);
+    }
+}
+
+// 电机堵转检测
+static void f_PID_ErrorHandle(PIDInstance *pid)
+{
+    /*Motor Blocked Handle*/
+    if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
+        return;
+
+    if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
+    {
+        // Motor blocked counting
+        pid->ERRORHandler.ERRORCount++;
+    }
+    else
+    {
+        pid->ERRORHandler.ERRORCount = 0;
+    }
+
+    if (pid->ERRORHandler.ERRORCount > 500)
+    {
+        // Motor blocked over 1000times
+        pid->ERRORHandler.ERRORType = PID_MOTOR_BLOCKED_ERROR;
+    }
+}
+
+/* ---------------------------下面是PID的外部算法接口--------------------------- */
+
+/**
+ * @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
+ *
+ * @param pid    PID实例
+ * @param config PID初始化设置
+ */
+void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
+{
+    // config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
+    // @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
+    // 后续修改为逐个赋值
+    memset(pid, 0, sizeof(PIDInstance));
+    // utilize the quality of struct that its memeory is continuous
+    memcpy(pid, config, sizeof(PID_Init_Config_s));
+    // set rest of memory to 0
+    DWT_GetDeltaT(&pid->DWT_CNT);
+}
+
+/**
+ * @brief          PID计算
+ * @param[in]      PID结构体
+ * @param[in]      测量值
+ * @param[in]      期望值
+ * @retval         返回空
+ */
+float PIDCalculate(PIDInstance *pid, float measure, float ref)
+{
+    // 堵转检测
+    if (pid->Improve & PID_ErrorHandle)
+        f_PID_ErrorHandle(pid);
+
+    pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
+
+    // 保存上次的测量值和误差,计算当前error
+    pid->Measure = measure;
+    pid->Ref = ref;
+    pid->Err = pid->Ref - pid->Measure;
+
+    // 如果在死区外,则计算PID
+    if (abs(pid->Err) > pid->DeadBand)
+    {
+        // 基本的pid计算,使用位置式
+        pid->Pout = pid->Kp * pid->Err;
+        pid->ITerm = pid->Ki * pid->Err * pid->dt;
+        pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
+
+        // 梯形积分
+        if (pid->Improve & PID_Trapezoid_Intergral)
+            f_Trapezoid_Intergral(pid);
+        // 变速积分
+        if (pid->Improve & PID_ChangingIntegrationRate)
+            f_Changing_Integration_Rate(pid);
+        // 微分先行
+        if (pid->Improve & PID_Derivative_On_Measurement)
+            f_Derivative_On_Measurement(pid);
+        // 微分滤波器
+        if (pid->Improve & PID_DerivativeFilter)
+            f_Derivative_Filter(pid);
+        // 积分限幅
+        if (pid->Improve & PID_Integral_Limit)
+            f_Integral_Limit(pid);
+
+        pid->Iout += pid->ITerm; // 累加积分
+        pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
+
+        // 输出滤波
+        if (pid->Improve & PID_OutputFilter)
+            f_Output_Filter(pid);
+
+        // 输出限幅
+        f_Output_Limit(pid);
+    }
+    else // 进入死区, 则清空积分和输出
+    {
+        pid->Output = 0;
+        pid->ITerm = 0;
+    }
+
+    // 保存当前数据,用于下次计算
+    pid->Last_Measure = pid->Measure;
+    pid->Last_Output = pid->Output;
+    pid->Last_Dout = pid->Dout;
+    pid->Last_Err = pid->Err;
+    pid->Last_ITerm = pid->ITerm;
+
+    return pid->Output;
+}
--- a/User_Code/module/algorithm/controller/controller.h
+++ b/User_Code/module/algorithm/controller/controller.h
@@ -1,8 +1,136 @@
-//
-// Created by ASUS on 2025/11/15.
-//
+/**
+ ******************************************************************************
+ * @file	 controller.h
+ * @author  Wang Hongxi
+ * @version V1.1.3
+ * @date    2021/7/3
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#ifndef _CONTROLLER_H
+#define _CONTROLLER_H

-#ifndef TRONONEH7_SCAFFOLD_CONTROLLER_H
-#define TRONONEH7_SCAFFOLD_CONTROLLER_H
+#include "main.h"
+#include "stdint.h"
+#include "memory.h"
+#include "stdlib.h"
+#include "bsp_dwt.h"
+#include "arm_math.h"
+#include <math.h>

-#endif //TRONONEH7_SCAFFOLD_CONTROLLER_H
+#ifndef abs
+#define abs(x) ((x > 0) ? x : -x)
+#endif
+
+// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
+typedef enum
+{
+    PID_IMPROVE_NONE = 0b00000000, // 0000 0000
+    PID_Integral_Limit = 0b00000001, // 0000 0001
+    PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
+    PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
+    PID_Proportional_On_Measurement = 0b00001000, // 0000 1000
+    PID_OutputFilter = 0b00010000, // 0001 0000
+    PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
+    PID_DerivativeFilter = 0b01000000, // 0100 0000
+    PID_ErrorHandle = 0b10000000, // 1000 0000
+} PID_Improvement_e;
+
+/* PID 报错类型枚举*/
+typedef enum errorType_e
+{
+    PID_ERROR_NONE = 0x00U,
+    PID_MOTOR_BLOCKED_ERROR = 0x01U
+} ErrorType_e;
+
+typedef struct
+{
+    uint64_t ERRORCount;
+    ErrorType_e ERRORType;
+} PID_ErrorHandler_t;
+
+/* PID结构体 */
+typedef struct
+{
+    //---------------------------------- init config block
+    // config parameter
+    float Kp;
+    float Ki;
+    float Kd;
+    float MaxOut;
+    float DeadBand;
+
+    // improve parameter
+    PID_Improvement_e Improve;
+    float IntegralLimit; // 积分限幅
+    float CoefA; // 变速积分 For Changing Integral
+    float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
+    float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
+    float Derivative_LPF_RC; // 微分滤波器系数
+
+    //-----------------------------------
+    // for calculating
+    float Measure;
+    float Last_Measure;
+    float Err;
+    float Last_Err;
+    float Last_ITerm;
+
+    float Pout;
+    float Iout;
+    float Dout;
+    float ITerm;
+
+    float Output;
+    float Last_Output;
+    float Last_Dout;
+
+    float Ref;
+
+    uint32_t DWT_CNT;
+    float dt;
+
+    PID_ErrorHandler_t ERRORHandler;
+} PIDInstance;
+
+/* 用于PID初始化的结构体*/
+typedef struct // config parameter
+{
+    // basic parameter
+    float Kp;
+    float Ki;
+    float Kd;
+    float MaxOut; // 输出限幅
+    float DeadBand; // 死区
+
+    // improve parameter
+    PID_Improvement_e Improve;
+    float IntegralLimit; // 积分限幅
+    float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
+    float CoefB; // ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
+    float Output_LPF_RC; // RC = 1/omegac
+    float Derivative_LPF_RC;
+} PID_Init_Config_s;
+
+/**
+ * @brief 初始化PID实例
+ * @todo 待修改为统一的PIDRegister风格
+ * @param pid    PID实例指针
+ * @param config PID初始化配置
+ */
+void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
+
+/**
+ * @brief 计算PID输出
+ *
+ * @param pid     PID实例指针
+ * @param measure 反馈值
+ * @param ref     设定值
+ * @return float  PID计算输出
+ */
+float PIDCalculate(PIDInstance *pid, float measure, float ref);
+
+#endif
--- a/User_Code/module/algorithm/kalman_filter/kalman_filter.c
+++ b/User_Code/module/algorithm/kalman_filter/kalman_filter.c
@@ -1,5 +1,486 @@
-//
-// Created by ASUS on 2025/11/15.
-//
+/**
+ ******************************************************************************
+ * @file    kalman filter.c
+ * @author  Wang Hongxi
+ * @version V1.2.2
+ * @date    2022/1/8
+ * @brief   C implementation of kalman filter
+ ******************************************************************************
+ * @attention
+ * 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。
+ * This implementation of kalman filter can dynamically adjust dimension and
+ * value of matrix H R and K according to the measurement validity under any
+ * circumstance that the sampling rate of component sensors are different.
+ *
+ * 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
+ * 入传感器量测所对应的状态与这个量测的方式，详情请见例程
+ * Therefore, the initialization of matrix P, F, and Q is sometimes different
+ * from that of matrices H R. when initialization. Additionally, the corresponding
+ * state and the method of the measurement should be provided when initializing
+ * measurement vector z. For more details, please see the example.
+ *
+ * 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初
+ * 始化矩阵P那样用常规方式初始化z H R即可。
+ * If automatic adjustment is not required, assign zero to the UseAutoAdjustment
+ * and initialize z H R in the normal way as matrix P.
+ *
+ * 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼
+ * 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
+ * MeasuredVector and ControlVector are required to be updated in the sensor
+ * callback function. Integer 0 in measurement vector z indicates the invalidity
+ * of current measurement, so MeasuredVector and ControlVector will be reset
+ * (to 0) during each update.
+ *
+ * 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法
+ * 通过限制矩阵P最小值的方法，可有效抑制滤波器的过度收敛，详情请见例程。
+ * Additionally, the excessive convergence of matrix P will make filter incapable
+ * of adopting the slowly changing state. This implementation can effectively
+ * suppress filter excessive convergence through boundary limiting for matrix P.
+ * For more details, please see the example.
+ *
+ * @example:
+ * x =
+ *   |   height   |
+ *   |  velocity  |
+ *   |acceleration|
+ *
+ * KalmanFilter_t Height_KF;
+ *
+ * void INS_Task_Init(void)
+ * {
+ *     static float P_Init[9] =
+ *     {
+ *         10, 0, 0,
+ *         0, 30, 0,
+ *         0, 0, 10,
+ *     };
+ *     static float F_Init[9] =
+ *     {
+ *         1, dt, 0.5*dt*dt,
+ *         0, 1, dt,
+ *         0, 0, 1,
+ *     };
+ *     static float Q_Init[9] =
+ *     {
+ *         0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
+ *         0.5*dt*dt*dt,        dt*dt,         dt,
+ *         0.5*dt*dt,              dt,         1,
+ *     };
+ *
+ *     // 设置最小方差
+ *     static float state_min_variance[3] = {0.03, 0.005, 0.1};
+ *
+ *     // 开启自动调整
+ *     Height_KF.UseAutoAdjustment = 1;
+ *
+ *     // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度
+ *     static uint8_t measurement_reference[3] = {1, 1, 3}
+ *
+ *     static float measurement_degree[3] = {1, 1, 1}
+ *     // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下）
+ *       |1   0   0|
+ *       |1   0   0|
+ *       |0   0   1|
+ *
+ *     static float mat_R_diagonal_elements = {30, 25, 35}
+ *     //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下）
+ *       |30   0   0|
+ *       | 0  25   0|
+ *       | 0   0  35|
+ *
+ *     Kalman_Filter_Init(&Height_KF, 3, 0, 3);
+ *
+ *     // 设置矩阵值
+ *     memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
+ *     memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
+ *     memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
+ *     memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference));
+ *     memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
+ *     memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements));
+ *     memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance));
+ * }
+ *
+ * void INS_Task(void const *pvParameters)
+ * {
+ *     // 循环更新
+ *     Kalman_Filter_Update(&Height_KF);
+ *     vTaskDelay(ts);
+ * }
+ *
+ * // 测量数据更新应按照以下形式 即向MeasuredVector赋值
+ * void Barometer_Read_Over(void)
+ * {
+ *     ......
+ *     INS_KF.MeasuredVector[0] = baro_height;
+ * }
+ * void GPS_Read_Over(void)
+ * {
+ *     ......
+ *     INS_KF.MeasuredVector[1] = GPS_height;
+ * }
+ * void Acc_Data_Process(void)
+ * {
+ *     ......
+ *     INS_KF.MeasuredVector[2] = acc.z;
+ * }
+ ******************************************************************************
+ */

 #include "kalman_filter.h"
+
+uint16_t sizeof_float, sizeof_double;
+
+static void H_K_R_Adjustment(KalmanFilter_t *kf);
+
+/**
+ * @brief 初始化矩阵维度信息并为矩阵分配空间
+ *
+ * @param kf kf类型定义
+ * @param xhatSize 状态变量维度
+ * @param uSize 控制变量维度
+ * @param zSize 观测量维度
+ */
+void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize)
+{
+    sizeof_float = sizeof(float);
+    sizeof_double = sizeof(double);
+
+    kf->xhatSize = xhatSize;
+    kf->uSize = uSize;
+    kf->zSize = zSize;
+
+    kf->MeasurementValidNum = 0;
+
+    // measurement flags
+    kf->MeasurementMap = (uint8_t *) user_malloc(sizeof(uint8_t) * zSize);
+    memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize);
+    kf->MeasurementDegree = (float *) user_malloc(sizeof_float * zSize);
+    memset(kf->MeasurementDegree, 0, sizeof_float * zSize);
+    kf->MatR_DiagonalElements = (float *) user_malloc(sizeof_float * zSize);
+    memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize);
+    kf->StateMinVariance = (float *) user_malloc(sizeof_float * xhatSize);
+    memset(kf->StateMinVariance, 0, sizeof_float * xhatSize);
+    kf->temp = (uint8_t *) user_malloc(sizeof(uint8_t) * zSize);
+    memset(kf->temp, 0, sizeof(uint8_t) * zSize);
+
+    // filter data
+    kf->FilteredValue = (float *) user_malloc(sizeof_float * xhatSize);
+    memset(kf->FilteredValue, 0, sizeof_float * xhatSize);
+    kf->MeasuredVector = (float *) user_malloc(sizeof_float * zSize);
+    memset(kf->MeasuredVector, 0, sizeof_float * zSize);
+    kf->ControlVector = (float *) user_malloc(sizeof_float * uSize);
+    memset(kf->ControlVector, 0, sizeof_float * uSize);
+
+    // xhat x(k|k)
+    kf->xhat_data = (float *) user_malloc(sizeof_float * xhatSize);
+    memset(kf->xhat_data, 0, sizeof_float * xhatSize);
+    Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *) kf->xhat_data);
+
+    // xhatminus x(k|k-1)
+    kf->xhatminus_data = (float *) user_malloc(sizeof_float * xhatSize);
+    memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
+    Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *) kf->xhatminus_data);
+
+    if (uSize != 0)
+    {
+        // control vector u
+        kf->u_data = (float *) user_malloc(sizeof_float * uSize);
+        memset(kf->u_data, 0, sizeof_float * uSize);
+        Matrix_Init(&kf->u, kf->uSize, 1, (float *) kf->u_data);
+    }
+
+    // measurement vector z
+    kf->z_data = (float *) user_malloc(sizeof_float * zSize);
+    memset(kf->z_data, 0, sizeof_float * zSize);
+    Matrix_Init(&kf->z, kf->zSize, 1, (float *) kf->z_data);
+
+    // covariance matrix P(k|k)
+    kf->P_data = (float *) user_malloc(sizeof_float * xhatSize * xhatSize);
+    memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize);
+    Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *) kf->P_data);
+
+    // create covariance matrix P(k|k-1)
+    kf->Pminus_data = (float *) user_malloc(sizeof_float * xhatSize * xhatSize);
+    memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize);
+    Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *) kf->Pminus_data);
+
+    // state transition matrix F FT
+    kf->F_data = (float *) user_malloc(sizeof_float * xhatSize * xhatSize);
+    kf->FT_data = (float *) user_malloc(sizeof_float * xhatSize * xhatSize);
+    memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize);
+    memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize);
+    Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *) kf->F_data);
+    Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *) kf->FT_data);
+
+    if (uSize != 0)
+    {
+        // control matrix B
+        kf->B_data = (float *) user_malloc(sizeof_float * xhatSize * uSize);
+        memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
+        Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *) kf->B_data);
+    }
+
+    // measurement matrix H
+    kf->H_data = (float *) user_malloc(sizeof_float * zSize * xhatSize);
+    kf->HT_data = (float *) user_malloc(sizeof_float * xhatSize * zSize);
+    memset(kf->H_data, 0, sizeof_float * zSize * xhatSize);
+    memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize);
+    Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *) kf->H_data);
+    Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *) kf->HT_data);
+
+    // process noise covariance matrix Q
+    kf->Q_data = (float *) user_malloc(sizeof_float * xhatSize * xhatSize);
+    memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize);
+    Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *) kf->Q_data);
+
+    // measurement noise covariance matrix R
+    kf->R_data = (float *) user_malloc(sizeof_float * zSize * zSize);
+    memset(kf->R_data, 0, sizeof_float * zSize * zSize);
+    Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *) kf->R_data);
+
+    // kalman gain K
+    kf->K_data = (float *) user_malloc(sizeof_float * xhatSize * zSize);
+    memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
+    Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *) kf->K_data);
+
+    kf->S_data = (float *) user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
+    kf->temp_matrix_data = (float *) user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
+    kf->temp_matrix_data1 = (float *) user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
+    kf->temp_vector_data = (float *) user_malloc(sizeof_float * kf->xhatSize);
+    kf->temp_vector_data1 = (float *) user_malloc(sizeof_float * kf->xhatSize);
+    Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *) kf->S_data);
+    Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *) kf->temp_matrix_data);
+    Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *) kf->temp_matrix_data1);
+    Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *) kf->temp_vector_data);
+    Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *) kf->temp_vector_data1);
+
+    kf->SkipEq1 = 0;
+    kf->SkipEq2 = 0;
+    kf->SkipEq3 = 0;
+    kf->SkipEq4 = 0;
+    kf->SkipEq5 = 0;
+}
+
+void Kalman_Filter_Measure(KalmanFilter_t *kf)
+{
+    // 矩阵H K R根据量测情况自动调整
+    // matrix H K R auto adjustment
+    if (kf->UseAutoAdjustment != 0)
+        H_K_R_Adjustment(kf);
+    else
+    {
+        memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
+        memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
+    }
+
+    memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize);
+}
+
+void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf)
+{
+    if (!kf->SkipEq1)
+    {
+        if (kf->uSize > 0)
+        {
+            kf->temp_vector.numRows = kf->xhatSize;
+            kf->temp_vector.numCols = 1;
+            kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
+            kf->temp_vector1.numRows = kf->xhatSize;
+            kf->temp_vector1.numCols = 1;
+            kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1);
+            kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
+        }
+        else
+        {
+            kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus);
+        }
+    }
+}
+
+void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf)
+{
+    if (!kf->SkipEq2)
+    {
+        kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT);
+        kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus);
+        kf->temp_matrix.numRows = kf->Pminus.numRows;
+        kf->temp_matrix.numCols = kf->FT.numCols;
+        kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT
+        kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus);
+    }
+}
+
+void Kalman_Filter_SetK(KalmanFilter_t *kf)
+{
+    if (!kf->SkipEq3)
+    {
+        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)
+        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);
+    }
+}
+
+void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
+{
+    if (!kf->SkipEq4)
+    {
+        kf->temp_vector.numRows = kf->H.numRows;
+        kf->temp_vector.numCols = 1;
+        kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k)
+        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)
+        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))
+        kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
+    }
+}
+
+void Kalman_Filter_P_Update(KalmanFilter_t *kf)
+{
+    if (!kf->SkipEq5)
+    {
+        kf->temp_matrix.numRows = kf->K.numRows;
+        kf->temp_matrix.numCols = kf->H.numCols;
+        kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
+        kf->temp_matrix1.numCols = kf->Pminus.numCols;
+        kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H
+        kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1);
+        // temp_matrix1 = K(k)·H·P'(k)
+        kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P);
+    }
+}
+
+/**
+ * @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
+ *
+ * @param kf kf类型定义
+ * @return float* 返回滤波值
+ */
+float *Kalman_Filter_Update(KalmanFilter_t *kf)
+{
+    // 0. 获取量测信息
+    Kalman_Filter_Measure(kf);
+    if (kf->User_Func0_f != NULL)
+        kf->User_Func0_f(kf);
+
+    // 先验估计
+    // 1. xhat'(k)= A·xhat(k-1) + B·u
+    Kalman_Filter_xhatMinusUpdate(kf);
+    if (kf->User_Func1_f != NULL)
+        kf->User_Func1_f(kf);
+
+    // 预测更新
+    // 2. P'(k) = A·P(k-1)·AT + Q
+    Kalman_Filter_PminusUpdate(kf);
+    if (kf->User_Func2_f != NULL)
+        kf->User_Func2_f(kf);
+
+    if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
+    {
+        // 量测更新
+        // 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
+        Kalman_Filter_SetK(kf);
+
+        if (kf->User_Func3_f != NULL)
+            kf->User_Func3_f(kf);
+
+        // 融合
+        // 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k))
+        Kalman_Filter_xhatUpdate(kf);
+
+        if (kf->User_Func4_f != NULL)
+            kf->User_Func4_f(kf);
+
+        // 修正方差
+        // 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k)
+        Kalman_Filter_P_Update(kf);
+    }
+    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);
+    }
+
+    // 自定义函数,可以提供后处理等
+    if (kf->User_Func5_f != NULL)
+        kf->User_Func5_f(kf);
+
+    // 避免滤波器过度收敛
+    // suppress filter excessive convergence
+    for (uint8_t i = 0; i < kf->xhatSize; ++i)
+    {
+        if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
+            kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
+    }
+
+    memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize);
+
+    if (kf->User_Func6_f != NULL)
+        kf->User_Func6_f(kf);
+
+    return kf->FilteredValue;
+}
+
+static void H_K_R_Adjustment(KalmanFilter_t *kf)
+{
+    kf->MeasurementValidNum = 0;
+
+    memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
+    memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
+
+    // 识别量测数据有效性并调整矩阵H R K
+    // recognize measurement validity and adjust matrices H R K
+    memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
+    memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
+    for (uint8_t i = 0; i < kf->zSize; ++i)
+    {
+        if (kf->z_data[i] != 0)
+        {
+            // 重构向量z
+            // rebuild vector z
+            kf->z_data[kf->MeasurementValidNum] = kf->z_data[i];
+            kf->temp[kf->MeasurementValidNum] = i;
+            // 重构矩阵H
+            // rebuild matrix H
+            kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i];
+            kf->MeasurementValidNum++;
+        }
+    }
+    for (uint8_t i = 0; i < kf->MeasurementValidNum; ++i)
+    {
+        // 重构矩阵R
+        // rebuild matrix R
+        kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]];
+    }
+
+    // 调整矩阵维数
+    // adjust the dimensions of system matrices
+    kf->H.numRows = kf->MeasurementValidNum;
+    kf->H.numCols = kf->xhatSize;
+    kf->HT.numRows = kf->xhatSize;
+    kf->HT.numCols = kf->MeasurementValidNum;
+    kf->R.numRows = kf->MeasurementValidNum;
+    kf->R.numCols = kf->MeasurementValidNum;
+    kf->K.numRows = kf->xhatSize;
+    kf->K.numCols = kf->MeasurementValidNum;
+    kf->z.numRows = kf->MeasurementValidNum;
+}
--- a/User_Code/module/algorithm/kalman_filter/kalman_filter.h
+++ b/User_Code/module/algorithm/kalman_filter/kalman_filter.h
@@ -1,8 +1,135 @@
-//
-// Created by ASUS on 2025/11/15.
-//
+/**
+ ******************************************************************************
+ * @file    kalman filter.h
+ * @author  Wang Hongxi
+ * @version V1.2.2
+ * @date    2022/1/8
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#ifndef __KALMAN_FILTER_H
+#define __KALMAN_FILTER_H

-#ifndef TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
-#define TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
+// cortex-m4 DSP lib
+/*
+#define __CC_ARM    // Keil
+#define ARM_MATH_CM4
+#define ARM_MATH_MATRIX_CHECK
+#define ARM_MATH_ROUNDING
+#define ARM_MATH_DSP    // define in arm_math.h
+*/

-#endif //TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
+#include "stm32h723xx.h"
+#include "arm_math.h"
+//#include "dsp/matrix_functions.h"
+#include "math.h"
+#include "stdint.h"
+#include "stdlib.h"
+
+#ifndef user_malloc
+#ifdef _CMSIS_OS_H
+#define user_malloc pvPortMalloc
+#else
+#define user_malloc malloc
+#endif
+#endif
+
+// 若运算速度不够,可以使用q31代替f32,但是精度会降低
+#define mat arm_matrix_instance_f32
+#define Matrix_Init arm_mat_init_f32
+#define Matrix_Add arm_mat_add_f32
+#define Matrix_Subtract arm_mat_sub_f32
+#define Matrix_Multiply arm_mat_mult_f32
+#define Matrix_Transpose arm_mat_trans_f32
+#define Matrix_Inverse arm_mat_inverse_f32
+
+typedef struct kf_t
+{
+    float *FilteredValue;
+    float *MeasuredVector;
+    float *ControlVector;
+
+    uint8_t xhatSize;
+    uint8_t uSize;
+    uint8_t zSize;
+
+    uint8_t UseAutoAdjustment;
+    uint8_t MeasurementValidNum;
+
+    uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state
+    float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H
+    float *MatR_DiagonalElements; // 量测方差 variance for each measurement
+    float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence
+    uint8_t *temp;
+
+    // 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个
+    uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5;
+
+    // definiion of struct mat: rows & cols & pointer to vars
+    mat xhat; // x(k|k)
+    mat xhatminus; // x(k|k-1)
+    mat u; // control vector u
+    mat z; // measurement vector z
+    mat P; // covariance matrix P(k|k)
+    mat Pminus; // covariance matrix P(k|k-1)
+    mat F, FT; // state transition matrix F FT
+    mat B; // control matrix B
+    mat H, HT; // measurement matrix H
+    mat Q; // process noise covariance matrix Q
+    mat R; // measurement noise covariance matrix R
+    mat K; // kalman gain  K
+    mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1;
+
+    int8_t MatStatus;
+
+    // 用户定义函数,可以替换或扩展基准KF的功能
+    void (*User_Func0_f)(struct kf_t *kf);
+
+    void (*User_Func1_f)(struct kf_t *kf);
+
+    void (*User_Func2_f)(struct kf_t *kf);
+
+    void (*User_Func3_f)(struct kf_t *kf);
+
+    void (*User_Func4_f)(struct kf_t *kf);
+
+    void (*User_Func5_f)(struct kf_t *kf);
+
+    void (*User_Func6_f)(struct kf_t *kf);
+
+    // 矩阵存储空间指针
+    float *xhat_data, *xhatminus_data;
+    float *u_data;
+    float *z_data;
+    float *P_data, *Pminus_data;
+    float *F_data, *FT_data;
+    float *B_data;
+    float *H_data, *HT_data;
+    float *Q_data;
+    float *R_data;
+    float *K_data;
+    float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1;
+} KalmanFilter_t;
+
+extern uint16_t sizeof_float, sizeof_double;
+
+void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize);
+
+void Kalman_Filter_Measure(KalmanFilter_t *kf);
+
+void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf);
+
+void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf);
+
+void Kalman_Filter_SetK(KalmanFilter_t *kf);
+
+void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf);
+
+void Kalman_Filter_P_Update(KalmanFilter_t *kf);
+
+float *Kalman_Filter_Update(KalmanFilter_t *kf);
+
+#endif //__KALMAN_FILTER_H
--- a/User_Code/module/algorithm/user_lib/user_lib.h
+++ b/User_Code/module/algorithm/user_lib/user_lib.h
@@ -10,24 +10,14 @@
 *
 ******************************************************************************
 */
-#ifndef _USER_LIB_H
-#define _USER_LIB_H
+#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;
+#include "stm32h723xx.h"
+#include "arm_math.h"

 #ifndef user_malloc
 #ifdef _CMSIS_OS_H
@@ -37,6 +27,19 @@ extern uint8_t GlobalDebugMode;
 #endif
 #endif

+#define msin(x) (arm_sin_f32(x))
+#define mcos(x) (arm_cos_f32(x))
+
+typedef arm_matrix_instance_f32 mat;
+// 若运算速度不够,可以使用q31代替f32,但是精度会降低
+#define MatAdd arm_mat_add_f32
+#define MatSubtract arm_mat_sub_f32
+#define MatMultiply arm_mat_mult_f32
+#define MatTranspose arm_mat_trans_f32
+#define MatInverse arm_mat_inverse_f32
+
+void MatInit(mat *m, uint8_t row, uint8_t col);
+
 /* boolean type definitions */
 #ifndef TRUE
 #define TRUE 1 /**< boolean true  */
@@ -87,40 +90,9 @@ extern uint8_t GlobalDebugMode;
 #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);

@@ -142,24 +114,20 @@ 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
+float *Norm3d(float *v);
+
+float NormOf3d(float *v);
+
+void Cross3d(float *v1, float *v2, float *res);
+
+float Dot3d(float *v1, float *v2);
+
+float AverageFilter(float new_data, float *buf, uint8_t len);
+
 #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
--- a/User_Code/module/general_def.h
+++ b/User_Code/module/general_def.h
@@ -0,0 +1,17 @@
+#ifndef GENERAL_DEF_H
+#define GENERAL_DEF_H
+
+// 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
+
+#ifndef PI
+#define PI 3.1415926535f
+#endif
+#define PI2 (PI * 2.0f) // 2 pi
+
+#define RAD_2_DEGREE 57.2957795f    // 180/pi
+#define DEGREE_2_RAD 0.01745329252f // pi/180
+
+#define RPM_2_ANGLE_PER_SEC 6.0f       // ×360°/60sec
+#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
+
+#endif // !GENERAL_DEF_H
--- a/User_Code/module/periph/imu/BMI088driver.c
+++ b/User_Code/module/periph/imu/BMI088driver.c
@@ -5,7 +5,7 @@
 #include "bsp_log.h"
 #include <math.h>

-#pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
+// Todo: #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;
@@ -64,7 +64,9 @@ IMU_Data_t BMI088;
    }

 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)
@@ -138,7 +140,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
            bmi088->GyroOffset[2] = GzOFFSET;
            bmi088->gNorm = gNORM;
            bmi088->TempWhenCali = 40;
-            LOGERROR("[BMI088] Calibrate Failed! Use offline params");
+            // Todo: LOGERROR("[BMI088] Calibrate Failed! Use offline params");
            break;
        }

@@ -209,7 +211,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
                gyroDiff[1] > 0.5f || //0.15
                gyroDiff[2] > 0.5f) //0.15
            {
-                LOGWARNING("[bmi088] calibration was interrupted\n");
+                // LOGWARNING("[bmi088] calibration was interrupted\n");
                break;
            }

@@ -264,7 +266,7 @@ uint8_t bmi088_accel_init(void)
    // check the "who am I"
    if (res != BMI088_ACC_CHIP_ID_VALUE)
    {
-        LOGERROR("[bmi088] Can not read bmi088 acc chip id");
+        // Todo: LOGERROR("[bmi088] Can not read bmi088 acc chip id");
        return BMI088_NO_SENSOR;
    }

@@ -312,7 +314,7 @@ uint8_t bmi088_gyro_init(void)
    // check the "who am I"
    if (res != BMI088_GYRO_CHIP_ID_VALUE)
    {
-        LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
+        //Todo: LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
        return BMI088_NO_SENSOR;
    }

--- a/cmake/stm32cubemx/CMakeLists.txt
+++ b/cmake/stm32cubemx/CMakeLists.txt
@@ -23,6 +23,7 @@ set(MX_Include_Dirs
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32H7xx/Include
    ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/ARM/DSP/Inc
 )

 # STM32CubeMX generated application sources