robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加ekf

Browse Source
This commit is contained in:
Robofish 2026-02-02 20:39:50 +08:00
parent e914c4647c
commit f0b9fdb003
2 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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
)

21
User/task/atti_esit.c
View File

@ -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;