RM赛事C型板九轴IMU解算（3）（姿态融合算法）-编程实验室

姿态解算是将陀螺仪的角速度数据，加速度计的加速度计数据，磁力计的磁场数据进行融合，以解算出当前载体的姿态角。姿态解算算法的好坏

将影响到姿态角度的精度。我们以mahony算法为例，移植相关算法，创建姿态解算任务

这篇还是教大家如何移植，至于如何进行优化，大家自行决定，比如加上DMA，用FreeRTOS多线程，等等

首先是配置就把前面两篇文章的所有配置在CubeMX中配置，和并把前面用到的文件一直到新的KEIL中并进行添加

然后再加入这两个文件

然后再在这个文件目录下，找到这两文件，添加到你的keil工程中

然后打开工程

在main.c中添加这些头文件

#include "imu_temp_control_task.h" #include "BMI088driver.h" #include "pid.h" #include "bsp_imu_pwm.h" #include "ist8310driver.h" #include "ist8310driver_middleware.h" #include "MahonyAHRS.h" #include <math.h>

和这些宏定义和变量

#define IMU_temp_PWM(pwm) imu_pwm_set(pwm) //pwm???? #define TEMPERATURE_PID_KP 1600.0f //kp of temperature control PID #define TEMPERATURE_PID_KI 0.2f //ki of temperature control PID #define TEMPERATURE_PID_KD 0.0f //kd of temperature control PID #define TEMPERATURE_PID_MAX_OUT 4500.0f //max out of temperature control PID #define TEMPERATURE_PID_MAX_IOUT 4400.0f //max iout of temperature control PID extern SPI_HandleTypeDef hspi1; volatile uint8_t imu_start_flag = 0; uint16_t tempPWM; fp32 gyro[3], accel[3], temp;float mag[3]; //kp, ki,kd three params const fp32 imu_temp_PID[3] = {TEMPERATURE_PID_KP, TEMPERATURE_PID_KI, TEMPERATURE_PID_KD}; //pid struct pid_type_def imu_temp_pid; fp32 quat[4] = {0.0f, 0.0f, 0.0f, 0.0f}; fp64 INS_angle[3] = {0.0f, 0.0f, 0.0f};

然后成立这个函数进行四元数对角度的转化

void get_angle(fp32 q[4], fp64 *yaw, fp64 *pitch, fp64 *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.29578; *pitch = asinf(-2.0f*(q[1]*q[3]-q[0]*q[2]))*57.29578; *roll = atan2f(2.0f*(q[0]*q[1]+q[2]*q[3]),2.0f*(q[0]*q[0]+q[3]*q[3])-1.0f)*57.29578; }

然后进行这些初始化

HAL_TIM_PWM_Start(&htim10,TIM_CHANNEL_1); /* USER CODE BEGIN 2 */ PID_init(&imu_temp_pid, PID_POSITION, imu_temp_PID, TEMPERATURE_PID_MAX_OUT, TEMPERATURE_PID_MAX_IOUT); ist8310_init(); while(BMI088_init()) { ; } //set spi frequency hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; if (HAL_SPI_Init(&hspi1) != HAL_OK) { Error_Handler(); } quat[0] = 1.0f; quat[1] = 0.0f; quat[2] = 0.0f; quat[3] = 0.0f;

最后替换掉原来的while（1）循环

while (1) { BMI088_read(gyro, accel, &temp); ist8310_read_mag(mag); // uint16_t tempPWM; PID_calc(&imu_temp_pid, temp, 40.0f); if (imu_temp_pid.out < 0.0f) { imu_temp_pid.out = 0.0f; } tempPWM = (uint16_t)imu_temp_pid.out; IMU_temp_PWM(tempPWM); MahonyAHRSupdate(quat, gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], mag[0], mag[1], mag[2]); get_angle(quat, INS_angle + 0, INS_angle + 1, INS_angle + 2); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ }

最后只需要读取INS_angle[3]的三个数据就分别是，Yao,Pitch,Roll的角度

但是我们这个姿态的解算没有进行滤波，也没用DMA所以数据的时效性不强，所以大家也可以根据大疆的例程进行修改