[51 MCU Quick Start Guide] 4.3.3: MPU6050 uses Mahony AHRS algorithm to implement six-axis attitude fusion to obtain quaternion and Euler angle

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 MPU6050 attitude solver - Mahony complementary filter - uppercase lowercase letters

       Automatic processing of input data range has been added, so that the calculation can be performed correctly even if the range is changed.

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

       Kp and Ki should be adjusted as needed. I use 1.5 and 0.005 here.

Mahony_6.c

#include

#include "MPU6050.h"

#define G 9.80665f // m/s^2

#define PI 3.141592653589793

idata float halfT = 1;

idata float GYRO_K = 1, ACCEL_K = 1;

void MPU6050_Mahony_Init(float loop_ms)

{

halfT = loop_ms/1000./2; //Calculate half of the cycle, unit s

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;

}

switch((MPU_Read_Byte(MPU_ACCEL_CFG_REG) >> 3) & 3)

{

case 0:

ACCEL_K = G/16384;

break;

case 1:

ACCEL_K = G/8192;

break;

case 2:

ACCEL_K = G/4096;

break;

case 3:

ACCEL_K = G/2048;

break;

}

}

static float invSqrt(float x) //Quickly calculate 1/Sqrt(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;

}

#define Kp 1.50f

#define Ki 0.005f

idata float Pitch, Roll, Yaw;

idata float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //quaternion

idata float exInt = 0, eyInt = 0, ezInt = 0; //Cumulative integral of cross product calculation error

void Imu_Update(float gx, float gy, float gz, float ax, float ay, float az)

{

unsigned char i;

float vx, vy, vz; //actual gravity acceleration

float ex, ey, ez; //Error in cross product calculation

float norm;

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;

float q0q3 = q0*q3;

float q1q1 = q1*q1;

float q1q2 = q1*q2;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

//Convert the original AD value of acceleration to m/s^2

ax = ax * ACCEL_K;

ay = ay * ACCEL_K;

az = az * ACCEL_K;

//Convert the gyroscope AD value to radians/s

gx = gx * GYRO_K;

gy = gy * GYRO_K;

gz = gz * GYRO_K;

if (ax * ay * az == 0)

return;

//The direction of gravity measured by the accelerometer (body coordinate system)

norm = invSqrt(ax * ax + ay * ay + az * az); // Previously it was written as invSqrt(ax*ax + ay+ay + az*az) which is wrong. Now it is corrected

ax = ax * norm;

ay = ay * norm;

az = az * norm;

//Actual gravity direction derived from quaternion (body coordinate system)

vx = 2 * (q1q3 - q0q2);

vy = 2 * (q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3;

//Cross product error

ex = (ay * vz - az * vy);

ey = (az * vx - ax * vz);

ez = (ax * vy - ay * vx);

//Cross product error integrated as angular velocity

exInt = exInt + ex * Ki;

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

//Angular velocity compensation

gx = gx + Kp * ex + exInt;

gy = gy + Kp * ey + eyInt;

gz = gz + Kp * ez + ezInt;

//Update quaternion

q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;

q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;

q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;

q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

//Unitized quaternion

norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 * norm;

q1 = q1 * norm;

q2 = q2 * norm;

q3 = q3 * norm;

//Quaternion inverse Euler angle

Yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3) * 57.3f;

Pitch = -asin(2.f * (q1q3 - q0q2)) * 57.3f;

Roll = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3) * 57.3f;

}

Mahony_6.h

#ifndef Mahony_6_H_

#define Mahony_6_H_

extern idata float Pitch, Roll, Yaw;

extern idata float q0, q1, q2, q3; //quaternion

void MPU6050_Mahony_Init(float loop_ms);

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

#endif

Instructions

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

Then use the Imu_Update posture fusion function.

test program

main.c

#include

#include "intrins.h"

#include "stdint.h"

#include "USART.h"

#include "./MPU6050/MPU6050.h"

#include "./MPU6050/Mahony_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)

{

int16_t aacx,aacy,aacz; //acceleration sensor raw data

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_Mahony_Init(85);

while(1)

{

MPU_Get_Accelerometer(&aacx, &aacy, &aacz); //Get accelerometer sensor data

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

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

printf("%f, ", Pitch);

printf("%f, ", Roll);

printf("%frn", Yaw);

}

}

Effect

Personally, I feel that on 51, the effect of Mahony is much better than that of DMP and other filtering algorithms. If the zero bias is processed, the effect will be even better.