【51单片机快速入门指南】4.4.3：Madgwick AHRS 九轴姿态融合获取四元数、欧拉角

Madgwick_9.h

#ifndef Madgwick_9_H_

#define Madgwick_9_H_

extern float Pitch, Roll, Yaw;

extern float q0, q1, q2, q3;

void MPU6050_Madgwick_Init(float loop_ms);

void MadgwickAHRSupdate_6(float gx, float gy, float gz, float ax, float ay, float az);

void MadgwickAHRSupdate_9(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);

#endif

使用方法

先调用MPU6050_Madgwick_Init(dt)，参数为一次循环的时间，单位为ms

再使用MadgwickAHRSupdate_9姿态融合函数。

测试

陀螺仪、磁力计的原始数据经校准后输入MadgwickAHRSupdate_9函数

main.c

#include

#include "intrins.h"

#include "stdint.h"

#include "USART.h"

#include "./Software_I2C/Software_I2C.h"

#include "XMC5883L.h"

#include "./MPU6050/MPU6050.h"

#include "./MPU6050/Madgwick_9.h"

void Delay1ms() //@22.1184MHz

{

unsigned char i, j;

_nop_();

_nop_();

i = 22;

j = 128;

do

{

while (--j);

} while (--i);

}

void delay_ms(uint32_t ms)

{

while(ms --)

Delay1ms();

}

#define LED_PORT P0

void main(void)

{

int16_t mag_x, mag_y, mag_z;

int16_t aacx,aacy,aacz; //加速度传感器原始数据

int16_t gyrox,gyroy,gyroz; //陀螺仪原始数据

MPU_Init();

xmc5883lInit();

AUXR &= 0xBF; //定时器时钟12T模式 1T的51使用12T的定时器程序时需要加入这两句

AUXR &= 0xFE; //串口1选择定时器1为波特率发生器

USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 22118400, 115200, DOUBLE_BAUD_ENABLE, USART_TIMER_1);

MPU6050_Madgwick_Init(10.48);

while(1)

{

MPU_Get_Accelerometer(&aacx, &aacy, &aacz); //得到加速度传感器数据

MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //得到陀螺仪数据

xmc5883lRead(&mag_x, &mag_y, &mag_z);

MadgwickAHRSupdate_9(gyrox+7, gyroy+23, gyroz-1, aacx, aacy, aacz, 1.108270606866881 * (mag_x + 297.2882033958856), 0.9218994400020794 * (mag_y + 3088.0092054124193), 0.9871899380641738 * (mag_z + 782.925290575134));

printf("%f, ", Pitch);

printf("%f, ", Roll);

printf("%frn", Yaw);

}

}

效果