目錄

• 源碼
• • Mahony_6.c
  • Mahony_6.h
• 使用方法
• 測試程序
• • main.c
  • 效果

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

---

???????移植自MPU6050姿態解算——Mahony互補濾波 —— 大寫的小寫字母

???????加入了輸入數據范圍的自動處理，即使更改量程也能正確解算。

源碼

???????為了避免所用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陀螺儀的原始數據

???????Kp和Ki要按需調整，我這里取1.5和0.005

Mahony_6.c

#include <math.h> #include "MPU6050.h"#define G 9.80665f // m/s^2#define PI 3.141592653589793idata float halfT = 1; idata float GYRO_K = 1, ACCEL_K = 1;void MPU6050_Mahony_Init(float loop_ms) {halfT = loop_ms/1000./2; //計算周期的一半，單位sswitch((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) //快速計算 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.005fidata float Pitch, Roll, Yaw; idata float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //四元數 idata float exInt = 0, eyInt = 0, ezInt = 0; //叉積計算誤差的累計積分void Imu_Update(float gx, float gy, float gz, float ax, float ay, float az) {unsigned char i;float vx, vy, vz; //實際重力加速度float ex, ey, ez; //叉積計算的誤差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;//將加速度原始AD值轉換為m/s^2ax = ax * ACCEL_K;ay = ay * ACCEL_K;az = az * ACCEL_K;//將陀螺儀AD值轉換為 弧度/sgx = gx * GYRO_K;gy = gy * GYRO_K;gz = gz * GYRO_K;if (ax * ay * az == 0)return;//加速度計測量的重力方向(機體坐標系)norm = invSqrt(ax * ax + ay * ay + az * az); //之前這里寫成invSqrt(ax*ax + ay+ay + az*az)是錯誤的，現在修改過來了ax = ax * norm;ay = ay * norm;az = az * norm;//四元數推出的實際重力方向(機體坐標系)vx = 2 * (q1q3 - q0q2);vy = 2 * (q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3;//叉積誤差ex = (ay * vz - az * vy);ey = (az * vx - ax * vz);ez = (ax * vy - ay * vx);//叉積誤差積分為角速度exInt = exInt + ex * Ki;eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;//角速度補償gx = gx + Kp * ex + exInt;gy = gy + Kp * ey + eyInt;gz = gz + Kp * ez + ezInt;//更新四元數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;//單位化四元數norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 * norm;q1 = q1 * norm;q2 = q2 * norm;q3 = q3 * norm;//四元數反解歐拉角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; //四元數void MPU6050_Mahony_Init(float loop_ms); void Imu_Update(float gx, float gy, float gz, float ax, float ay, float az);#endif

使用方法

先調用MPU6050_Mahony_Init(dt)，參數為一次循環的時間，單位為ms
再使用Imu_Update姿態融合函數。

測試程序

main.c

#include <STC89C5xRC.H> #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; //加速度傳感器原始數據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_Mahony_Init(85);while(1){ MPU_Get_Accelerometer(&aacx, &aacy, &aacz); //得到加速度傳感器數據MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz); //得到陀螺儀數據Imu_Update(gyrox, gyroy, gyroz, aacx, aacy, aacz);printf("%f, ", Pitch);printf("%f, ", Roll);printf("%f\r\n", Yaw);} }

效果

???????個人感覺在51上，Mahony的效果要遠遠好于DMP及其他濾波算法的表現，如果把零偏處理了，效果會更好。

總結

以上是生活随笔為你收集整理的【51单片机快速入门指南】4.3.3： MPU6050使用Mahony AHRS算法实现六轴姿态融合获取四元数、欧拉角的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。