From f0b9fdb003152900361f76476427f7841dba810b Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Mon, 2 Feb 2026 20:39:50 +0800
Subject: [PATCH] =?UTF-8?q?=E5=8A=A0ekf?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CMakeLists.txt        |  2 ++
 User/task/atti_esit.c | 21 ++++++++++++++++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 34b894a..2b06102 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -68,6 +68,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/component/pid.c
     User/component/user_math.c
     User/component/vmc.c
+    User/component/QuaternionEKF.c
 
     # User/device sources
     User/device/bmi088.c
@@ -119,6 +120,7 @@ list(REMOVE_ITEM CMAKE_C_IMPLICIT_LINK_LIBRARIES ob)
 # Add linked libraries
 target_link_libraries(${CMAKE_PROJECT_NAME}
     stm32cubemx
+    ${CMAKE_SOURCE_DIR}/Drivers/CMSIS/DSP/Lib/GCC/libarm_cortexM7lfsp_math.a
 
     # Add user defined libraries
 )
diff --git a/User/task/atti_esit.c b/User/task/atti_esit.c
index 770756e..0af843e 100644
--- a/User/task/atti_esit.c
+++ b/User/task/atti_esit.c
@@ -10,6 +10,7 @@
 #include "bsp/mm.h"
 #include "bsp/pwm.h"
 #include "component/ahrs.h"
+#include "component/QuaternionEKF.h"
 #include "component/pid.h"
 #include "device/bmi088.h"
 #include "module/balance_chassis.h"
@@ -18,13 +19,18 @@
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
+/* 姿态解算算法选择: 定义USE_QEKF使用EKF算法，注释掉则使用AHRS算法 */
+#define USE_QEKF
+
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
 BMI088_t bmi088;
 
+#ifndef USE_QEKF
 AHRS_t gimbal_ahrs;
 AHRS_Magn_t magn;
+#endif
 AHRS_Eulr_t eulr_to_send;
 Chassis_IMU_t imu_for_chassis;
 KPID_t imu_temp_ctrl_pid;
@@ -55,7 +61,11 @@ void Task_atti_esit(void *argument) {
 
   /* USER CODE INIT BEGIN */
   BMI088_Init(&bmi088, &cali_bmi088);
+#ifdef USE_QEKF
+  QEKF_Init(10.0f, 0.001f, 1000000.0f, 0.9996f, 0.0f);
+#else
   AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
+#endif
   PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
            1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
   BSP_PWM_Start(BSP_PWM_IMU_HEAT);
@@ -78,10 +88,19 @@ void Task_atti_esit(void *argument) {
     // IST8310_Parse(&ist8310);
 
     /* 根据设备接收到的数据进行姿态解析 */
+#ifdef USE_QEKF
+    QEKF_Update(bmi088.gyro.x, bmi088.gyro.y, bmi088.gyro.z,
+                bmi088.accl.x, bmi088.accl.y, bmi088.accl.z,
+                1.0f / BMI088_GetUpdateFreq(&bmi088));
+    /* 从EKF获取欧拉角 */
+    eulr_to_send.rol = QEKF_INS.roll;
+    eulr_to_send.pit = QEKF_INS.pitch;
+    eulr_to_send.yaw = QEKF_INS.yaw;
+#else
     AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
-
     /* 根据解析出来的四元数计算欧拉角 */
     AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+#endif
     /* 加速度计数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
     imu_for_chassis.accl.x = bmi088.accl.y;
     imu_for_chassis.accl.y = -bmi088.accl.x;