123G_MPU6050_Oled_Test.zip_MPU_123_STM32 MPU6050_mpu6050与OLED_ol

#include <stdint.h> #include <stdbool.h> #include "inc/hw_i2c.h" #include "inc/hw_memmap.h" #include "driverlib/debug.h" #include "driverlib/gpio.h" #include "driverlib/sysctl.h" #include "driverlib/pin_map.h" #include "driverlib/i2c.h" #include "mpu9250.h" #include "math.h" MPU9250_STRUCT mpu9250; int mpu9250_ready; enum { Acc_x = 0, Acc_y , Acc_z , Temp, Gyro_x , Gyro_y , Gyro_z , Mag_x, Mag_y, Mag_z, }; #define Kp 0.3f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.001f // 0.001 integral gain governs rate of convergence of gyroscope biases float To_180_degrees(float x) { return (x>180?(x-360):(x<-180?(x+360):x)); } #define OFFSET_SAMP_NUM 500 //校准偏移量时的平均次数。 #define MAGOFFSET_SAMP_NUM 1000 //校准偏移量时的平均次数。 #define FILTER_N 10 float GY91_filter_buf[10][FILTER_N + 1] = {0}; long filter_sum[10] = {0}; float ref_q[4] = {1,0,0,0}; float norm_acc,norm_q; float norm_acc_lpf; xyz_float reference_v; ref_t ref; long sum_temp[10]= {0}; unsigned int acc_sum_cnt = 0,gyro_sum_cnt = 0,mag_sum_cnt = 0; #define my_pow(a) ((a)*(a)) #define TO_M_S2 0.23926f // 980cm/s2 +-8g 980/4096 #define ANGLE_TO_RADIAN 0.01745329f //*0.01745 = /57.3 角度转弧度 #define IMU_INTEGRAL_LIM ( 2.0f *ANGLE_TO_RADIAN ) #define MAX_ACC 4096.0f //+-8G 加速度计量程 #define TO_DEG_S 500.0f float mag_norm ,mag_norm_xyz ; xyz_float mag_sim_3d,acc_3d_hg,acc_ng,acc_ng_offset; int acc_ng_cali;//T = 2ms 默认为2ms ，数值等于1/T //************************************** //MPU9250 //************************************** void InitMPU9250(void) { Single_WriteI2C(GYRO_ADDRESS,PWR_MGMT_1, 0x00);//解除休眠状态 SysCtlDelay(1000); Single_WriteI2C(GYRO_ADDRESS,CONFIG, 0x04);//使能20hz低通滤波输出 SysCtlDelay(1000); Single_WriteI2C(GYRO_ADDRESS,SMPLRT_DIV, 0x00);//1000hz SysCtlDelay(1000); Single_WriteI2C(GYRO_ADDRESS,GYRO_CONFIG, 0x18);//2000DPS SysCtlDelay(1000); Single_WriteI2C(GYRO_ADDRESS,ACCEL_CONFIG, 0x10);//8g SysCtlDelay(1000); Single_WriteI2C(GYRO_ADDRESS,ByPass_CONFIG, 0x02);//turn on Bypass Mode SysCtlDelay(1000); Single_WriteI2C(MAG_ADDRESS,AK8975_CNTL, 0x01);//TRIG MAG SAMPLE SysCtlDelay(1000); Single_WriteI2C(MAG_ADDRESS,AK8975_ASAX, 135);//TRIG MAG SAMPLE SysCtlDelay(1000); Single_WriteI2C(MAG_ADDRESS,AK8975_ASAY, 154);//TRIG MAG SAMPLE SysCtlDelay(1000); Single_WriteI2C(MAG_ADDRESS,AK8975_ASAZ, 128);//TRIG MAG SAMPLE SysCtlDelay(1000); if(Single_ReadI2C(GYRO_ADDRESS,WHO_AM_I)==WHO_AM_I_VALUE){ mpu9250.acc_offset_flag=0; mpu9250.gyro_offset_flag=0; mpu9250.mag_offset_flag=0; mpu9250.Acc_Offset.x=46.30910119; mpu9250.Acc_Offset.y=-1.644884115; mpu9250.Acc_Offset.z=-475.4423968; mpu9250.Gyro_Offset.x=16.25698135; mpu9250.Gyro_Offset.y=22.65505936; mpu9250.Gyro_Offset.z=2.448728095; mpu9250.Mag_Offset.x=9.5; mpu9250.Mag_Offset.y=-7.5; mpu9250.Mag_Offset.z=-35; SysCtlDelay(10000); GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_2, 0);//点亮LED灯 } else{ mpu9250_ready=0; SysCtlDelay(10000); GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_2, GPIO_PIN_2);//熄灭LED灯 } } void Single_WriteI2C(unsigned int SlaveAddres,unsigned int REG_Address,unsigned int REG_data) { I2CMasterSlaveAddrSet(I2C0_BASE, SlaveAddres, false);//false表示发数据，0x68是mpu6050/mpu9250的地址 I2CMasterDataPut(I2C0_BASE, REG_Address);//发送一个0x75数据，这是mpu6050/mpu9250的WHO_AM_I寄存器地址 I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START); while(I2CMasterBusy(I2C0_BASE));//等待数据发送完毕 I2CMasterDataPut(I2C0_BASE, REG_data);//发送一个0x75数据，这是mpu6050/mpu9250的WHO_AM_I寄存器地址 I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH); while(I2CMasterBusy(I2C0_BASE));//等待数据发送完毕 } //************************************** int Single_ReadI2C(unsigned int SlaveAddres,unsigned int REG_Address) { int REG_data=0; I2CMasterSlaveAddrSet(I2C0_BASE, SlaveAddres, false);//false表示发数据，0x68是mpu6050/mpu9250的地址 I2CMasterDataPut(I2C0_BASE, REG_Address);//发送一个0x75数据，这是mpu6050/mpu9250的WHO_AM_I寄存器地址 I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_SINGLE_SEND); unsigned int I2C_isBusBusycount=0; while(I2CMasterBusy(I2C0_BASE)){ I2C_isBusBusycount++; SysCtlDelay(1); if(I2C_isBusBusycount>900){//I2C_isBusBusycount典型值78 break; } };//等待数据发送完毕 I2CMasterSlaveAddrSet(I2C0_BASE, SlaveAddres, true);//收数据 I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_SINGLE_RECEIVE); I2C_isBusBusycount=0; while(I2CMasterBusy(I2C0_BASE)){ I2C_isBusBusycount++; SysCtlDelay(1); if(I2C_isBusBusycount>900){//I2C_isBusBusycount典型值78 break; } };//等待数据发送完毕 REG_data = I2CMasterDataGet(I2C0_BASE);//读取数据 return REG_data; } void continual_ReadMPU9250(){ unsigned int REG_data[14]={0}; I2CMasterSlaveAddrSet(I2C0_BASE, 0x68, false);//false表示发数据，0x68是mpu6050/mpu9250的地址 I2CMasterDataPut(I2C0_BASE, 0x3B);//发送一个0x75数据，这是mpu6050/mpu9250的WHO_AM_I寄存器地址 I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_SINGLE_SEND); unsigned int I2C_isBusBusycount=0; while(I2CMasterBusy(I2C0_BASE)){ I2C_isBusBusycount++; SysCtlDelay(1); if(I2C_isBusBusycount>90){//I2C_isBusBusycount典型值78 break; } };//等待数据发送完毕 I2CMasterSlaveAddrSet(I2C0_BASE, 0x68, true);//收数据 I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START); unsigned int i =0; for(i=0;i<14;i++){ if(i==0) I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START); else if(i==13) I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH); else I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT); unsigned int I2C_isBusBusycount=0; while(I2CMasterBusy(I2C0_BASE)){ I2C_isBusBusycount++; SysCtlDelay(1); if(I2C_isBusBusycount>90){//I2C_isBusBusycount典型值78 break; } };//等待数据发送完毕 REG_data[i] = I2CMasterDataGet(I2C0_BASE);//读取数据 } mpu9250.Acc_I16.x = ((REG_data[0]<<8)+REG_data[1]); mpu9250.Acc_I16.y = ((REG_data[2]<<8)+REG_data[3]); mpu9250.Acc_I16.z = -((REG_data[4]<<8)+REG_data[5]);//因为mpu9250是反着放的 mpu9250.temp = ((REG_data[6]<<8)+REG_data[7]); mpu9250.Gyro_I16.x = ((REG_data[8]<<8)+REG_data[9]); mpu9250.Gyro_I16.y = ((REG_data[10]<<8)+REG_data[11]); mpu9250.Gyro_I16.z = ((REG_data[12]<<8)+REG_data[13]); } //void continual_ReadAK8975(){ // unsigned int REG_data[6]={0}; // if(I2C_isBusBusy(I2CA_BASE)){ // if((I2C_getStatus(I2CA_BASE) & I2C_STS_NO_ACK) == 1)//发送无应答 // I2C_clearStatus(I2CA_BASE, I2C_STS_NO_ACK); // I2C_sendStopCondition(I2CA_BASE);//发送前保证总线空闲 // } // I2C_setSlaveAddress(I2CA_BASE, MAG_ADDRESS); // I2C_setConfig(I2CA_BASE, I2C_MASTER_SEND_MODE); // I2C_setDataCount(I2CA_BASE, 1); // I2C_putData(I2CA_BASE,AK8975_HXL); // I2C_sendStartCondition(I2CA_BASE);//发送起始地址位 // int I2C_STS_REG_ACCESS_RDYcount=0; // while((I2C_getStatus(I2CA_BASE) & I2C_STS_REG_ACCES