void MPU6050_Read(MPU6050_DATA *sensor_data)
{
// 温度
mpu6050_buffer[0] = MPU6050_ReadByte(MPU6050_Addr, TEMP_OUT_L);
mpu6050_buffer[1] = MPU6050_ReadByte(MPU6050_Addr, TEMP_OUT_H);
Temperature = (mpu6050_buffer[1] << 8) | mpu6050_buffer[0]; //16位数据
// 陀螺仪
mpu6050_buffer[2] = MPU6050_ReadByte(MPU6050_Addr, GYRO_XOUT_L);
mpu6050_buffer[3] = MPU6050_ReadByte(MPU6050_Addr, GYRO_XOUT_H);
sensor_data->gx = (mpu6050_buffer[3] << 8) | mpu6050_buffer[2];
sensor_data->gx -= MPU6050_OFFSER.gx ;//
mpu6050_buffer[4] = MPU6050_ReadByte(MPU6050_Addr, GYRO_YOUT_L);
mpu6050_buffer[5] = MPU6050_ReadByte(MPU6050_Addr, GYRO_YOUT_H);
sensor_data->gy = (mpu6050_buffer[5] << 8) | mpu6050_buffer[4];
sensor_data->gy -= MPU6050_OFFSER.gy;//
mpu6050_buffer[6] = MPU6050_ReadByte(MPU6050_Addr, GYRO_ZOUT_L);
mpu6050_buffer[7] = MPU6050_ReadByte(MPU6050_Addr, GYRO_ZOUT_H);
sensor_data->gz = (mpu6050_buffer[7] << 8) | mpu6050_buffer[6];
sensor_data->gz -= MPU6050_OFFSER.gz;//
// 加速度计
mpu6050_buffer[8] = MPU6050_ReadByte(MPU6050_Addr, ACCEL_XOUT_L);
mpu6050_buffer[9] = MPU6050_ReadByte(MPU6050_Addr, ACCEL_XOUT_H);
sensor_data->ax = (mpu6050_buffer[9] << 8) | mpu6050_buffer[8];
sensor_data->ax -= MPU6050_OFFSER.ax;//
mpu6050_buffer[10] = MPU6050_ReadByte(MPU6050_Addr, ACCEL_YOUT_L);
mpu6050_buffer[11] = MPU6050_ReadByte(MPU6050_Addr, ACCEL_YOUT_H);
sensor_data->ay = (mpu6050_buffer[11] << 8) | mpu6050_buffer[10];
sensor_data->ay -= MPU6050_OFFSER.ay;//
mpu6050_buffer[12]=MPU6050_ReadByte(MPU6050_Addr, ACCEL_ZOUT_L);
mpu6050_buffer[13]=MPU6050_ReadByte(MPU6050_Addr, ACCEL_ZOUT_H);
sensor_data->az = (mpu6050_buffer[13] << 8) | mpu6050_buffer[10];
sensor_data->az -= MPU6050_OFFSER.az;//
/******去除零漂*********/
if(GYRO_OFFSET_OK)
{
static int32_t tempgx=0,tempgy=0,tempgz=0;
static uint8_t cnt_g=0;
static int i = 0;
if(cnt_g==0)
{
MPU6050_OFFSER.gx=0;
MPU6050_OFFSER.gy=0;
MPU6050_OFFSER.gz=0;
tempgx = 0;
tempgy = 0;
tempgz = 0;
cnt_g = 1;
return;
}
tempgx+= sensor_data->gx;
tempgy+= sensor_data->gy;
tempgz+= sensor_data->gz;
if(cnt_g==200)
{
MPU6050_OFFSER.gx = tempgx/cnt_g;//49
MPU6050_OFFSER.gy = tempgy/cnt_g;//14
MPU6050_OFFSER.gz = tempgz/cnt_g;//-10
cnt_g = 0;
GYRO_OFFSET_OK = 0;
EE_SAVE_GYRO_OFFSET();//保存数据
for(i=0;i<3;i++)
{all_ON(); delay_ms(300);all_OFF();delay_ms(300);}
return;
}
cnt_g++;
}
if(ACC_OFFSET_OK)
{
static int32_t tempax=0,tempay=0,tempaz=0;
static uint8_t cnt_a=0;
static int i =0;
if(cnt_a==0)
{
MPU6050_OFFSER.ax = 0;
MPU6050_OFFSER.ay = 0;
MPU6050_OFFSER.az = 0;
tempax = 0;
tempay = 0;
tempaz = 0;
cnt_a = 1;
return;
}
tempax+= sensor_data->ax;
tempay+= sensor_data->ay;
tempaz+= (sensor_data->az - 16384);
if(cnt_a==200)
{
MPU6050_OFFSER.ax = tempax/cnt_a;
MPU6050_OFFSER.ay = tempay/cnt_a;
MPU6050_OFFSER.az = tempaz/cnt_a;
cnt_a = 0;
ACC_OFFSET_OK = 0;
EE_SAVE_ACC_OFFSET();//保存数据
for(i=0;i<3;i++)
{all_ON(); delay_ms(300);all_OFF();delay_ms(300);}
return;
}
cnt_a++;
}
}
/**************************实现函数********************************************
//将iic读取到得数据分拆,放入相应寄存器
*******************************************************************************/
void MPU6050_Dataanl(void)
{
MPU6050_ACC_LAST.X=((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) - ACC_OFFSET.X; //减去零偏
MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) - ACC_OFFSET.Y;
MPU6050_ACC_LAST.Z=((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]) - ACC_OFFSET.Z;
//跳过温度ADC
MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]) - GYRO_OFFSET.X;
MPU6050_GYRO_LAST.Y=((((int16_t)mpu6050_buffer[10]) << 8) | mpu6050_buffer[11]) - GYRO_OFFSET.Y;
MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12]) << 8) | mpu6050_buffer[13]) - GYRO_OFFSET.Z;
if(!GYRO_OFFSET_OK) //进行零偏计算
{
static int32_t tempgx=0,tempgy=0,tempgz=0;
static uint8_t cnt_g=0;
// LED1_ON;
if(cnt_g==0)
{
GYRO_OFFSET.X=0;
GYRO_OFFSET.Y=0;
GYRO_OFFSET.Z=0;
tempgx = 0;
tempgy = 0;
tempgz = 0;
cnt_g = 1; //防止以后清零静态变量
return;
}
tempgx+= MPU6050_GYRO_LAST.X;
tempgy+= MPU6050_GYRO_LAST.Y;
tempgz+= MPU6050_GYRO_LAST.Z;
if(cnt_g==200)
{
GYRO_OFFSET.X=tempgx/cnt_g;
GYRO_OFFSET.Y=tempgy/cnt_g;
GYRO_OFFSET.Z=tempgz/cnt_g;
cnt_g = 0;
GYRO_OFFSET_OK = 1;
EE_SAVE_GYRO_OFFSET();//保存数据
return;
}
cnt_g++;
}
if(!ACC_OFFSET_OK)
{
static int32_t tempax=0,tempay=0,tempaz=0;
static uint8_t cnt_a=0;
// LED1_ON;
if(cnt_a==0)
{
ACC_OFFSET.X = 0;
ACC_OFFSET.Y = 0;
ACC_OFFSET.Z = 0;
tempax = 0;
tempay = 0;
tempaz = 0;
cnt_a = 1;
return;
}
tempax+= MPU6050_ACC_LAST.X;
tempay+= MPU6050_ACC_LAST.Y;
//tempaz+= MPU6050_ACC_LAST.Z;
if(cnt_a==200)
{
ACC_OFFSET.X=tempax/cnt_a;
ACC_OFFSET.Y=tempay/cnt_a;
ACC_OFFSET.Z=tempaz/cnt_a;
cnt_a = 0;
ACC_OFFSET_OK = 1;
EE_SAVE_ACC_OFFSET();//保存数据
return;
}
cnt_a++;
}
}
