RM赛事C型板九轴IMU解算（3）（姿态融合算法）

姿态解算是将陀螺仪的角速度数据，加速度计的加速度计数据，磁力计的磁场数据进行融合，以解算出当前载体的姿态角。姿态解算算法的好坏
将影响到姿态角度的精度。我们以 mahony 算法为例，移植相关算法，创建姿态解算任务

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

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

然后再加入这两个文件

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

然后打开工程

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

cpp 复制代码

#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>

和这些宏定义和变量

cpp 复制代码

#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};

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

cpp 复制代码

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

然后进行这些初始化

cpp 复制代码

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）循环

cpp 复制代码

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所以数据的时效性不强，所以大家也可以根据大疆的例程进行修改