MPU6050+HMC5883+BMP180+GPS导航系统设计

老师有个项目，让我搞惯导这一块,虽然最后也没有用上廉价的MPU6050，而是用了一两万的Xsens。但是本人还是想写一下MPU6050，虽然技术含量不高，但是写下来，留个纪念吧。

首先是MPU6050，属于MEMS传感器，现在做四轴的用这款芯片的很多，它集成了三轴陀螺仪+三轴加速度计，因为官方提供DMP库，这样就大大方便了开发，使用硬解，解放主控资源。自己买回来MPU6050后，测了其静态漂移。把MPU6050模块插在面包板上，微处理器用的是stm32，通讯方式是IIC，姿态解算用的是硬件解算，用的例程是原子的，然后通过每隔一段时间记录一次,mpu6050自带的DMP进行解算，直接输出四元数，然后通过四元数转化转换为对应的姿态角，但是没有Z轴上的参考量，所以Yaw是不准的。(Time是次数的意思，当时的时间间隔忘记了）

发现半个小时，yaw漂6--7度。对于微型小四轴来说够用了，一般微型小四轴也就飞7、8分钟左右。通过mpu6050，我们可以得到roll，pitch，yaw。至于mpu6050的源码，大家自己看原子的。这里就不放了。

其次是HMC5883，它是磁力计，可以测量x,y,z方向上的磁场。它与微处理器也是通过IIC通讯。至于它的有关介绍可以看DATASHEET。其代码如下：

#include "HMC5883L.h"
#include "sys.h"
#include "delay.h"
#include "usart.h" 
#define SlaveAddress 0x3c

void Init_HMC5883(void)
{
	      MPU_IIC_Init();
	      Write_HMC5883_Byte(0x02,0x00);		
}

u8 Write_HMC5883_Byte(u8 add, u8 da)
{
   MPU_IIC_Start();                 
   MPU_IIC_Send_Byte(SlaveAddress);   
	 if(MPU_IIC_Wait_Ack())	
	{
		MPU_IIC_Stop();		 
		return 1;		
	}	
   MPU_IIC_Send_Byte(add);   
	 MPU_IIC_Wait_Ack();	

   MPU_IIC_Send_Byte(da);     
	if(MPU_IIC_Wait_Ack())
{
		MPU_IIC_Stop();	 
		return 1;		 
	}		

   MPU_IIC_Stop();	
	 return 0;
}
u8 Read_HMC5883_Byte(u8 REG_Address)
{   
	u8 REG_data;
   MPU_IIC_Start();                        
   MPU_IIC_Send_Byte(SlaveAddress);           
   MPU_IIC_Wait_Ack();

    MPU_IIC_Send_Byte(REG_Address);                   	
	  MPU_IIC_Wait_Ack();

   MPU_IIC_Start();                        
   MPU_IIC_Send_Byte(SlaveAddress+1);         
	 MPU_IIC_Wait_Ack();	

    REG_data=MPU_IIC_Read_Byte(0);            
	  MPU_IIC_Stop();                         
    return REG_data; 
}
void Multiple_read_HMC5883(u8*BUF)
{   u8 i;
    MPU_IIC_Start();                          
    MPU_IIC_Send_Byte(SlaveAddress);           
	  MPU_IIC_Wait_Ack();
    MPU_IIC_Send_Byte(0x03);                   	
	  MPU_IIC_Wait_Ack();
    MPU_IIC_Start();                          
    MPU_IIC_Send_Byte(SlaveAddress+1);        
	  MPU_IIC_Wait_Ack();
	 for (i=0; i<6; i++)                     
    {
        
        if (i == 5)
        {
           BUF[i] = MPU_IIC_Read_Byte(0);          
        }
        else
        {
          BUF[i] = MPU_IIC_Read_Byte(1);          
       }
   }
    MPU_IIC_Stop();                          
}

接下来是BMP180气压计，它通过测气压得到相对海拔高度，但是我感觉噪声有点大，估计我用的是原始值，没有滤波的原因，大家可以用MS5611，精度比较高。效果会好些。但是气压计这个东西本来就会受环境影响。它也是通过IIC与微处理器通讯的，我直接上代码：

#include "BMP180.h"
#include "delay.h"
#include "mpuiic.h"
#include "sys.h"
long  result_UT=0;
long  result_UP=0;

short ac1;
short ac2; 
short ac3; 
unsigned short ac4;
unsigned short ac5;
unsigned short ac6;
short b1; 
short b2;
short mb;
short mc;
short md;
u16 Multiple_read(u8 ST_Address)
{   
	u8 msb, lsb;
	u16 _data;
    MPU_IIC_Start();                          
    MPU_IIC_Send_Byte(BMP180_SlaveAddress);    
	MPU_IIC_Wait_Ack();
    MPU_IIC_Send_Byte(ST_Address);            
	MPU_IIC_Wait_Ack();                                
    MPU_IIC_Start();                          
    MPU_IIC_Send_Byte(BMP180_SlaveAddress+1);        
	MPU_IIC_Wait_Ack();                                       

    msb = MPU_IIC_Read_Byte(1);                                
    lsb = MPU_IIC_Read_Byte(0);              

    MPU_IIC_Stop();                         
    delay_ms(5);
    _data = msb << 8;
	_data |= lsb;	
	return _data;
}
//********************************************************************
u16 bmp180ReadTemp(void)
{

    MPU_IIC_Start();                  
    MPU_IIC_Send_Byte(BMP180_SlaveAddress);   
	MPU_IIC_Wait_Ack();
    MPU_IIC_Send_Byte(0xF4);	        
    while(MPU_IIC_Wait_Ack());
	MPU_IIC_Send_Byte(0x2E);       	
    while(MPU_IIC_Wait_Ack());
	MPU_IIC_Stop();                   
	delay_ms(10);	
	return Multiple_read(0xF6);
}
//*************************************************************
u16 bmp180ReadPressure(void)
{
	//u16 pressure = 0;

    MPU_IIC_Start();                  
    MPU_IIC_Send_Byte(BMP180_SlaveAddress);  
    while(MPU_IIC_Wait_Ack());
	MPU_IIC_Send_Byte(0xF4);	         
    while(MPU_IIC_Wait_Ack());
	MPU_IIC_Send_Byte(0x34);       	  
    while(MPU_IIC_Wait_Ack());
	MPU_IIC_Stop();                   
	delay_ms(20);    	                  
	

	
	return Multiple_read(0xF6);//pressure;	
}

//**************************************************************


void Init_BMP180()
{
	MPU_IIC_Init();
	ac1 = Multiple_read(0xAA);
	ac2 = Multiple_read(0xAC);
	ac3 = Multiple_read(0xAE);
	ac4 = Multiple_read(0xB0);
	ac5 = Multiple_read(0xB2);
	ac6 = Multiple_read(0xB4);
	b1 =  Multiple_read(0xB6);
	b2 =  Multiple_read(0xB8);
	mb =  Multiple_read(0xBA);
	mc =  Multiple_read(0xBC);
	md =  Multiple_read(0xBE);
}
//***********************************************************************
void bmp180Convert()
{
	unsigned int ut;
	unsigned long up;
	long x1, x2, b5, b6, x3, b3, p;
	unsigned long b4, b7;

	ut = bmp180ReadTemp();	 
	up = bmp180ReadPressure();  
	//*************
	x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
	x2 = ((long) mc << 11) / (x1 + md);
	b5 = x1 + x2;
	result_UT = ((b5 + 8) >> 4);
	//*************		
	b6 = b5 - 4000;
	                        
	x1 = (b2 * (b6 * b6)>>12)>>11;
	x2 = (ac2 * b6)>>11;
	x3 = x1 + x2;
	b3 = (((((long)ac1)*4 + x3)<>2;	
	                         
	x1 = (ac3 * b6)>>13;
	x2 = (b1 * ((b6 * b6)>>12))>>16;
	x3 = ((x1 + x2) + 2)>>2;
	b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;
	
	b7 = ((unsigned long)(up - b3) * (50000>>OSS));
	if (b7 < 0x80000000)
	p = (b7<<1)/b4;
	else
	p = (b7/b4)<<1;
	
	x1 = (p>>8) * (p>>8);
	x1 = (x1 * 3038)>>16;
	x2 = (-7357 * p)>>16;
	result_UP = p+((x1 + x2 + 3791)>>4);
}

GPS是通过USART通信的，代码就不放了。

这些数据都是直接采的，没进行滤波什么的，姿态解算也是用的硬解。然后自己用VB写了个上位机，将所有原始数据传给上位机。

除此之外，我将mpu6050的数据通过串口传给Arduino,然后主要想做个3D姿态跟随的效果，Arduino将数据传给电脑，然后实现3d姿态仿真的效果。后来发现其实labview也可以做3d仿真。

以上内容，难免有错误与不足之处，大家踊跃拍砖。