[51 MCU Quick Start Guide] 4.3.4: MPU6050 uses Madgwick AHRS algorithm to achieve six-axis attitude fusion to obtain quaternion,

STC89C516 32MHz

Keil uVision V5.29.0.0

PK51 Prof. Developers Kit Version:9.60.0.0

Host computer: Vofa+ 1.3.10

Transplanted from AHRS - LOXO, algorithm author: SOH Madgwick

Source code

       In order to avoid exceeding the RAM limit, some variables are set to idata type. Please pay attention to this when porting.

       The MCU used is STC89C516 crystal oscillator 16MHz 6T mode

       stdint.h See [51 MCU Quick Start Guide] 1: Basic Knowledge and Project Creation

       For software I2C program, see [51 MCU Quick Start Guide] 4: Software I2C

       For the serial port part, see [51 MCU Quick Start Guide] 3.3: USART Serial Port Communication

       MPU6050.c, MPU6050.h, see [51 MCU Quick Start Guide] 4.3: I2C reads the raw data of the MPU6050 gyroscope

        Beta needs to be adjusted as needed, I use 0.1 here

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

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

//Convert the gyroscope AD value to radians/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

//====================================================================================================

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

Instructions

First call MPU6050_Madgwick_Init(dt), the parameter is the time of one cycle, the unit is ms

Then use the MadgwickAHRSupdate_6 attitude fusion function.

test program

main.c

#include

#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; //acceleration sensor raw data

idata int16_t gyrox,gyroy,gyroz; //gyroscope raw data

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); //Get accelerometer sensor data

MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //Get gyroscope data

MadgwickAHRSupdate_6(gyrox, gyroy, gyroz, aacx, aacy, aacz);

printf("%f, ", Pitch);

printf("%f, ", Roll);

printf("%frn", Yaw);

}

}

Effect