//得到陀螺仪值(原始值)
//（gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)）
//原函数表述如上，原gx为short, 现改为float
//返回值:0,成功
// 其他,错误代码
uint8_t MPU_Get_Gyroscope(float *gx,float *gy,float *gz)
{
uint8_t buf[6],res;
short Gx, Gy, Gz;
static float gx_off=0;
static float gy_off=0;
float sum_gx=0;
float sum_gy=0;
static uint8_t flag=0;
if(flag == 0)
{
for(uint16_t i=0; i<400; i++)
{
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
Gx=((uint16_t)buf[0]<<8)|buf[1];
Gy=((uint16_t)buf[2]<<8)|buf[3];
Gz=((uint16_t)buf[4]<<8)|buf[5];
// *gx = Gx / 16.4 +3.12;
// *gy = Gy / 16.4 + 1;
*gx = Gx / 16.4;
*gy = Gy / 16.4;
sum_gx+=*gx;
sum_gy+=*gy;
}
}
gx_off=sum_gx/400.0f;
gy_off=sum_gy/400.0f;
flag = 1;
}
else
{
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
Gx=((uint16_t)buf[0]<<8)|buf[1];
Gy=((uint16_t)buf[2]<<8)|buf[3];
Gz=((uint16_t)buf[4]<<8)|buf[5];
// *gx = Gx / 16.4 +3.12;
// *gy = Gy / 16.4 + 1;
*gx = Gx / 16.4 - gx_off;
*gy = Gy / 16.4 - gy_off;
insert(&Queue_gx, *gx); //之前对PITCH值做200点滑动滤波发现响应不好，现对陀螺仪角速度做10点滑动滤波
insert(&Queue_gy, *gy); //
*gx=MovAve(&Queue_gx);
*gy=MovAve(&Queue_gy);
}
}
return res;;
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
uint8_t buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((uint16_t)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
uint8_t buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((uint16_t)buf[0]<<8)|buf[1];
*ay=((uint16_t)buf[2]<<8)|buf[3];
*az=((uint16_t)buf[4]<<8)|buf[5];
}
return res;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
void MPU6050_Read(void)
{
MPU_Read_Len(MPU_ADDR ,MPU_ACCEL_XOUTH_REG,14, mpu6050_buffer);
}
