mpu6050 文件

来源：互联网发布：revit 数据分离插件编辑：程序博客网时间：2024/05/22 13:21

/*----------------------------------------------------------------------------*/
/*
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);

}