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陀螺仪的原始数据
Mahony_6.c
#include <math.h>
#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; //计算周期的一半,单位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) //快速计算 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; //四元数
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^2
ax = ax * ACCEL_K;
ay = ay * ACCEL_K;
az = az * ACCEL_K;
//将陀螺仪AD值转换为 弧度/s
gx = 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
测试程序
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及其他滤波算法的表现,如果把零偏处理了,效果会更好。