mpu6050 文件

/*----------------------------------------------------------------------------*/
/*
 File Name   :   MPU6050c

    Description   :

    Author    :   Jones.Lee
 Copyright   :  Jones_Workspace
    Version&Date  :  <2013.5.1>
    Connector Fuction  :
    Notes ：
*/          
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/

    /*---Head Files---*/

 #include "mpu6050.h"
 #include "i2c.h"
 #include "math.h"
 #include "uart.h"
 #include "VisualScope.h"
    /*---Variable Definition--*/

    /*---Function Definiton---*/

    /*---Macro Definition-----*/

  #define  GREATH_DATA    3.9     //  2/0xff

 #define  GYRO_LSB  16.4

 #define  DEG_RAD   57.32   //180/ 3.14

 #define  ACCEL_X_REVISE  0
 #define  ACCEL_Y_REVISE  0
 #define  ACCEL_Z_REVISE  0

 #define  ANGLE_X_REVISE  0
 #define  ANGLE_Y_REVISE  0
 #define  ANGLE_Z_REVISE  0

 #define  GYRO_X_REVISE  90
 #define  GYRO_Y_REVISE  0
 #define  GYRO_Z_REVISE  0
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
/*
Fuction Name      ：  Mpu6050Init

Description       ：

Input parameter   ：    Class   Name  Action 
       
Output parameter  ：    Class   Name  Action 
     
Author&Date       ： Jones.Lee & 2013.5.1

Notes             ： 
*/
/*----------------------------------------------------------------------------*/
void Mpu6050Init()
{
 IIC_Write_Byte(MPU_ADDR,PWR_MGMT_1, 0x00);
 IIC_Write_Byte(MPU_ADDR,SMPLRT_DIV, 0x07);
 IIC_Write_Byte(MPU_ADDR,CONFIG, 0x06);
 IIC_Write_Byte(MPU_ADDR,GYRO_CONFIG, 0x18);
 IIC_Write_Byte(MPU_ADDR,ACCEL_CONFIG, 0x01);

}
/*----------------------------------------------------------------------------*/
/*
Fuction Name      ：  Mpu6050_ReadAccelrate

Description       ：

Input parameter   ：    Class   Name  Action 
       
Output parameter  ：    Class   Name  Action 
     
Author&Date       ： Jones.Lee & 2013.5.1

Notes             ： there is a problem that I deal with it. the problem is
      about the value
*/
/*----------------------------------------------------------------------------*/
void Mpu6050_ReadAccelrate(unsigned short int* pwAccelX,unsigned short int* pwAccelY, unsigned short int * pwAccelZ)
{
    unsigned char MSub;
    unsigned char byData[6];
    unsigned int wTemp;

    MSub =  ACCEL_XOUT_H;
   
    IIC_Read_Bytes(MPU_ADDR, MSub, byData, 6);
    wTemp  = 0;
    wTemp = byData[0] << 8;
    wTemp |= byData[1];
 * pwAccelX = wTemp;

    wTemp  = 0;
    wTemp = byData[2] << 8;
    wTemp |= byData[3];
 * pwAccelY = wTemp;
   
    wTemp  = 0;
    wTemp = byData[4] << 8;
    wTemp |= byData[5];
 * pwAccelZ = wTemp;

#if  1      // 零点漂移

 *pwAccelX +=ACCEL_X_REVISE; 
 *pwAccelY +=ACCEL_Y_REVISE;
 *pwAccelZ +=ACCEL_Z_REVISE;
#endif
#if  1     

 *pwAccelX =- *pwAccelX ; 
 *pwAccelY =- *pwAccelY;
 *pwAccelZ =- *pwAccelZ ;
#endif
}
/*----------------------------------------------------------------------------*/
/*
Fuction Name      ： Mpu6050_ReadAngle

Description       ：

Input parameter   ：    Class   Name  Action 
       float *   pwAngleX  the point of x axis's angle
      float *   pwAngleY  the point of y axis's angle
      float *   pwAngleZ  the point of z axis's angle
Output parameter  ：    Class   Name  Action 
      void
Author&Date       ： Jones.Lee & 2013.3.16

Notes             ： 
*/
/*----------------------------------------------------------------------------*/
void Mpu6050_ReadAngle(float * pwAngleX,float *pwAngleY, float *pwAngleZ)
{
 unsigned char i;

 unsigned short int mid[3];    //y有个问题 就是变量转移问题 很是问题
           //这是折中办法  和 前面的角速度一样
    short int mids[3];

 double midf[3];

 unsigned int greath;

 unsigned short int m;

 greath = greath;       //消除Warning

 m = m;
 Mpu6050_ReadAccelrate(mid,mid+1,mid+2);
 for(i = 0; i < 3; i++)
 {
  mids[i] = mid[i];
  midf[i] = mids[i];
  midf[i] = ((midf[i]*GREATH_DATA));     // GREATH_DATA表示 3.9mg/LSB
 }
 
  
#if  0           //这种算法很简单，但是不好。有误差

 greath = sqrt(pow(midf[0],2)+pow(midf[1],2)+pow(midf[2],2));
 *pwAngleX =asin(midf[0]/greath);      //x轴的偏
 *pwAngleY =asin(midf[1]/greath);
 *pwAngleZ =asin(midf[2]/greath);
 
#else           //这种算法很复杂，建立模型，但是很简单，我套用公式，也懂了

 *pwAngleX = atan( midf[0] / ( sqrt( pow( midf[1] , 2 ) + pow( midf[2] ,2 ))));//x轴的偏角
 *pwAngleY = atan( midf[1] / ( sqrt( pow( midf[0] , 2 ) + pow( midf[2] , 2))));
 *pwAngleZ = atan( ( sqrt( pow( midf[0] , 2 ) + pow( midf[1] , 2))) / midf[2]);
#endif

#ifdef  DEG_RAD      //转化为角度。
 *pwAngleX *=DEG_RAD; 
 *pwAngleY *=DEG_RAD;
 *pwAngleZ *=DEG_RAD;
#endif

#if  1    
 *pwAngleX =-*pwAngleX; 
 *pwAngleY =-*pwAngleY;
 *pwAngleZ =-*pwAngleZ;
#endif
#if  0     
 *pwAngleX +=ANGLE_X_REVISE; 
 (*pwAngleY)=(*pwAngleY)-20;
 *pwAngleZ +=ANGLE_Z_REVISE;
#endif

}
/*----------------------------------------------------------------------------*/
/*
Fuction Name      ： Read_Gyro

Description       ：

Input parameter   ：    Class   Name  Action 
       float *   pwGyroX  the point of x axis's gyro
      float *   pwGyroY  the point of y axis's gyro
      float *   pwGyroZ  the point of z axis's gyro
Output parameter  ：    Class   Name  Action 
      void
Author&Date       ： Jones.Lee & 2013.3.16

Notes             ： 
*/
/*----------------------------------------------------------------------------*/
void Mpu6050_ReadGyro(float * pwGyroX,float * pwGyroY, float * pwGyroZ)
{
    unsigned char MSub;
    unsigned char byData[6];
    unsigned int wTemp;
 short int gyro[3];
 float  fgyro[3];
    MSub = GYRO_XOUT_H;
   
    IIC_Read_Bytes(MPU_ADDR, MSub, byData, 2);
    wTemp  = 0;
    wTemp = byData[0] << 8;
    wTemp |= byData[1];
    gyro[0] = wTemp;

    wTemp  = 0;
    wTemp = byData[2] << 8;
    wTemp |= byData[3];
    gyro[1]  = wTemp;
   
    wTemp  = 0;
    wTemp = byData[4] << 8;
    wTemp |= byData[5];
    gyro[2] = wTemp;

 fgyro[0] = gyro[0];
 fgyro[1] = gyro[1];
 fgyro[2] = gyro[2];
#if  1     

 fgyro[0] +=GYRO_X_REVISE; 
 fgyro[1] +=GYRO_Y_REVISE;
 fgyro[2] +=GYRO_Z_REVISE;
#endif

#ifdef    GYRO_LSB
 *pwGyroX =fgyro[0]/(GYRO_LSB);
  *pwGyroY =fgyro[1]/(GYRO_LSB);
 *pwGyroZ =fgyro[2]/(GYRO_LSB);
#endif

// VisualScope_Display(gyro[0],*pwGyroX,0,0,1000,1,0);

}