//得到陀螺仪值(原始值) //(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); }