【51单片机快速入门指南】4.3.4： MPU6050使用Madgwick AHRS算法实现六轴姿态融合获取四元数、欧拉角_51单片机控制mpu6050

STC89C516 32MHz
Keil uVision V5.29.0.0
PK51 Prof.Developers Kit Version:9.60.0.0
上位机：Vofa+ 1.3.10

移植自AHRS —— LOXO，算法作者：SOH Madgwick

源码

       为了避免所用RAM超标，部分变量设为idata类型，移植时需注意。
       所用MCU为STC89C516 晶振16MHz 6T模式

       stdint.h见【51单片机快速入门指南】1：基础知识和工程创建
       软件I2C程序见【51单片机快速入门指南】4： 软件 I2C
       串口部分见【51单片机快速入门指南】3.3：USART 串口通信
       MPU6050.c、MPU6050.h见【51单片机快速入门指南】4.3： I2C读取MPU6050陀螺仪的原始数据

        beta要按需调整，我这里取0.1

Madgwick_6.c

//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files
#include <math.h>
#include "MPU6050.h"

//---------------------------------------------------------------------------------------------------
// Definitions

#define beta	0.1f										// 2 * proportional gain (Kp)

//---------------------------------------------------------------------------------------------------
// Variable definitions

idata volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
idata volatile float Pitch = 0.0f, Roll = 0.0f, Yaw = 0.0f;

//====================================================================================================
// Functions

idata float sampleFreq = 1;
idata float GYRO_K = 1;

void MPU6050_Madgwick_Init(float loop_ms)
{
	sampleFreq = 1000. / loop_ms;	//sample frequency in Hz
	switch((MPU_Read_Byte(MPU_GYRO_CFG_REG) >> 3) & 3)
	{
		case 0:
			GYRO_K = 1./131/57.3;
			break;
		case 1:
			GYRO_K = 1./65.5/57.3;
			break;
		case 2:
			GYRO_K = 1./32.8/57.3;
			break;
		case 3:
			GYRO_K = 1./16.4/57.3;
			break;
	}
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float x) 
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update
//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdate_6(float gx, float gy, float gz, float ax, float ay, float az) 
{
	float recipNorm;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

	//将陀螺仪AD值转换为 弧度/s
	gx = gx * GYRO_K;
	gy = gy * GYRO_K;
	gz = gz * GYRO_K;

	// Rate of change of quaternion from gyroscope
	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Auxiliary variables to avoid repeated arithmetic
		_2q0 = 2.0f * q0;
		_2q1 = 2.0f * q1;
		_2q2 = 2.0f * q2;
		_2q3 = 2.0f * q3;
		_4q0 = 4.0f * q0;
		_4q1 = 4.0f * q1;
		_4q2 = 4.0f * q2;
		_8q1 = 8.0f * q1;
		_8q2 = 8.0f * q2;
		q0q0 = q0 * q0;
		q1q1 = q1 * q1;
		q2q2 = q2 * q2;
		q3q3 = q3 * q3;

		// Gradient decent algorithm corrective step
		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
		s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
		s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
		s0 *= recipNorm;
		s1 *= recipNorm;
		s2 *= recipNorm;
		s3 *= recipNorm;

		// Apply feedback step
		qDot1 -= beta * s0;
		qDot2 -= beta * s1;
		qDot3 -= beta * s2;
		qDot4 -= beta * s3;
	}

	// Integrate rate of change of quaternion to yield quaternion
	q0 += qDot1 * (1.0f / sampleFreq);
	q1 += qDot2 * (1.0f / sampleFreq);
	q2 += qDot3 * (1.0f / sampleFreq);
	q3 += qDot4 * (1.0f / sampleFreq);

	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;

	Pitch = asin(-2.0f * (q1*q3 - q0*q2))* 57.3f;
	Roll = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) * 57.3f;
	Yaw = atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)* 57.3f;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156

Madgwick_6.h

#ifndef Madgwick_6_H_
#define Madgwick_6_H_

extern idata float Pitch, Roll, Yaw;
extern idata 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);

#endif
1
2
3
4
5
6
7
8
9
10

使用方法

先调用MPU6050_Madgwick_Init(dt)，参数为一次循环的时间，单位为ms
再使用MadgwickAHRSupdate_6姿态融合函数。

测试程序

main.c

#include <STC89C5xRC.H>
#include "intrins.h"
#include "stdint.h"
#include "USART.h"
#include "./MPU6050/MPU6050.h"
#include "./MPU6050/Madgwick_6.h"

void Delay1ms()		//@32MHz
{
	unsigned char i, j;

	i = 6;
	j = 44;
	do
	{
		while (--j);
	} while (--i);
}


void Delay_ms(int i)
{
	while(i--)
		Delay1ms();
}

void main(void)
{
	idata int16_t aacx,aacy,aacz;		//加速度传感器原始数据
	idata int16_t gyrox,gyroy,gyroz;	//陀螺仪原始数据

	USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 32000000, 4800, DOUBLE_BAUD_DISABLE, USART_TIMER_2);
	MPU_Init(); 
	MPU6050_Madgwick_Init(95);
	while(1)
	{	
		MPU_Get_Accelerometer(&aacx, &aacy, &aacz);	//得到加速度传感器数据
		MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);	//得到陀螺仪数据
		MadgwickAHRSupdate_6(gyrox, gyroy, gyroz, aacx, aacy, aacz);
		printf("%f, ", Pitch);
		printf("%f, ", Roll);
		printf("%f\r\n", Yaw);
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

效果