mpu9250驱动C语言源码 磁力计校准算法

#include "mpu9250.h" #include "delay.h" #include "struct_all.h" uint8_t ACC_GYRO_Offset1 = 0;//不自动校正 uint8_t MAG_Offset1=0,MAG_Offset2=0;//不自动校正 float Hxs=1.0f,Hys=1.0f,Hzs=1.0f; void Init_MPU9250(void) { I2C_ByteWrite(GYRO_ADDRESS,PWR_MGMT_1,0x00); delay_ms(4); I2C_ByteWrite(GYRO_ADDRESS,SMPLRT_DIV,0x00); delay_ms(4); I2C_ByteWrite(GYRO_ADDRESS,CONFIG,0x03); delay_ms(4); I2C_ByteWrite(GYRO_ADDRESS,GYRO_CONFIG,0x18);delay_ms(4); I2C_ByteWrite(GYRO_ADDRESS,ACCEL_CONFIG,0x18);delay_ms(4); I2C_ByteWrite(GYRO_ADDRESS,ACCEL_CONFIG_2,0x02);delay_ms(4); I2C_ByteWrite(GYRO_ADDRESS,0x37,0x02); delay_ms(4); I2C_ByteWrite(MAG_ADDRESS,0x0A,0x16); delay_ms(4); } /****************************************************************************** 函数原型： void Do_ACC_GYRO_Offset(void) 功 能： MPU9250零偏校正 *******************************************************************************/ void Do_ACC_GYRO_Offset(void) { ACC_GYRO_Offset1=1; } void Do_MAG_Offset(void) { MAG_Offset1=1; } void ACC_GYRO_Offset(void) { static int32_t ACC_X=0,ACC_Y=0,ACC_Z=0; static int32_t GYRO_X=0,GYRO_Y=0,GYRO_Z=0; static uint8_t count_acc_gyro=0; if(ACC_GYRO_Offset1) { if(count_acc_gyro==0) { LED2=1; conf.offset_acc.x = 0;conf.offset_gyro.x = 0; conf.offset_acc.y = 0;conf.offset_gyro.y = 0; conf.offset_acc.z = 0;conf.offset_gyro.z = 0; ACC_X = 0;GYRO_X = 0; ACC_Y = 0;GYRO_Y = 0; ACC_Z = 0;GYRO_Z = 0; count_acc_gyro = 1; } else { count_acc_gyro++; ACC_X += acc.x;GYRO_X += gyro.x; ACC_Y += acc.y;GYRO_Y += gyro.y; ACC_Z += acc.z;GYRO_Z += gyro.z; } if(count_acc_gyro==251) { count_acc_gyro--; conf.offset_acc.x = ACC_X/count_acc_gyro;conf.offset_gyro.x = GYRO_X/count_acc_gyro; conf.offset_acc.y = ACC_Y/count_acc_gyro;conf.offset_gyro.y = GYRO_Y/count_acc_gyro; conf.offset_acc.z = ACC_Z/count_acc_gyro-2048;conf.offset_gyro.z = GYRO_Z/count_acc_gyro; conf.angleTrim[ROLL]=0; conf.angleTrim[PITCH]=0; SPIROM_SAVE_PID_BOX(); count_acc_gyro = 0; ACC_GYRO_Offset1 = 0; LED2=0; } } } void MAG_Offset(void) { static int16_t X_max,X_min,Y_max,Y_min,Z_max,Z_min; static uint16_t count_mag=0; if(MAG_Offset1) { if(count_mag==0) { LED1=1; conf.offset_mag.x = 0;conf.offset_mag.y = 0; Hxs=1;Hys=1; count_mag=1; } else if(count_mag==1) { X_max=mag.x;X_min=mag.x;Y_max=mag.y;Y_min=mag.y; count_mag=2; } else { count_mag++; if(mag.x>X_max) X_max=mag.x; if(mag.x<X_min) X_min=mag.x; if(mag.y>Y_max) Y_max=mag.y; if(mag.y<Y_min) Y_min=mag.y; } if(count_mag==721) { Hxs= 1 ; Hys=(float)(X_max-X_min)/(Y_max-Y_min); conf.offset_mag.x =Hxs*(X_max+X_min)/2; conf.offset_mag.y =Hys*(Y_max+Y_min)/2; conf.data_y=Hys*100; count_mag=0;MAG_Offset1=0; LED1=0; delay_ms(5000); MAG_Offset2=1; } delay_ms(14); } if(MAG_Offset2) { if(count_mag==0) { LED0=0; conf.offset_mag.z = 0; Hzs=1; count_mag=1; } else if(count_mag==1) { Z_max=mag.z;Z_min=mag.z;Y_max=mag.y;Y_min=mag.y;count_mag=2;} else { count_mag++; if(mag.z>Z_max) Z_max=mag.z; if(mag.z<Z_min) Z_min=mag.z; if(mag.y>Y_max) Y_max=mag.y; if(mag.y<Y_min) Y_min=mag.y; } if(count_mag==721) { Hzs=(float)(Y_max-Y_min)/(Z_max-Z_min); conf.offset_mag.z =Hzs*(Z_max+Z_min)/2; conf.data_z=Hzs*100; SPIROM_SAVE_PID_BOX(); count_mag=0;MAG_Offset2=0; LED0=1; } delay_ms(14); } } void READ_MPU9250_ACCEL_GYRO(void) { static u8 tmpBuffer[14]; I2C_BufferRead(GYRO_ADDRESS, ACCEL_XOUT_H, tmpBuffer, 14); acc.x = ((((int16_t)tmpBuffer[0]) << 8) | tmpBuffer[1]) - conf.offset_acc.x; //减去零偏 acc.y = ((((int16_t)tmpBuffer[2]) << 8) | tmpBuffer[3]) - conf.offset_acc.y; acc.z = ((((int16_t)tmpBuffer[4]) << 8) | tmpBuffer[5]) - conf.offset_acc.z; gyro.x = ((((int16_t)tmpBuffer[8]) << 8)| tmpBuffer[9]) - conf.offset_gyro.x; gyro.y = ((((int16_t)tmpBuffer[10]) << 8)| tmpBuffer[11])- conf.offset_gyro.y; gyro.z = ((((int16_t)tmpBuffer[12]) << 8)| tmpBuffer[13])- conf.offset_gyro.z; ACC_GYRO_Offset(); } void READ_MPU9250_MAG(void) { static u8 tmpBuffer[7]; I2C_BufferRead(MAG_ADDRESS, MAG_XOUT_L, tmpBuffer, 7); if(!(tmpBuffer[6]&0x08)){ mag.x=(s16)((u16)tmpBuffer[1]<<8|tmpBuffer[0])*Hxs-conf.offset_mag.x; mag.y=(s16)((u16)tmpBuffer[3]<<8|tmpBuffer[2])*Hys-conf.offset_mag.y; mag.z=(s16)((u16)tmpBuffer[5]<<8|tmpBuffer[4])*Hzs-conf.offset_mag.z; MAG_Offset(); } }