MPU6050+HMC5883驱动程序

#include \#include \#include \

u8 mpu6050_buffer[14]; u8 hmc5883_buffer[6]; float HMC5883_Angle_F; u16 HMC5883_Angle_D; int16_t ax,ay,az;

S_INT16_XYZ GYRO_OFFSET,ACC_OFFSET; uint8_t GYRO_OFFSET_OK = 1; uint8_t ACC_OFFSET_OK = 1;

S_INT16_XYZ MPU6050_ACC_LAST,MPU6050_GYRO_LAST;

/**************************实现函数******************************************** //将iic读取到得数据分拆,放入相应寄存器

*******************************************************************************/

void MPU6050_Dataanl(void) { MPU6050_ACC_LAST.X=((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1])- 510; //ACC_OFFSET.X; MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3])- 42;//ACC_OFFSET.Y; MPU6050_ACC_LAST.Z=((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]);//- 16950;//ACC_OFFSET.Z; //跳过温度ADC MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9])-227;// - GYRO_OFFSET.X; MPU6050_GYRO_LAST.Y=((((int16_t)mpu6050_buffer[10]) << 8) | mpu6050_buffer[11])-18;// - GYRO_OFFSET.Y; MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12]) << 8) | mpu6050_buffer[13])+110;//+26; //- GYRO_OFFSET.Z; VIEW_16_1=(int16_t)MPU6050_ACC_LAST.X; VIEW_16_2=(int16_t)MPU6050_ACC_LAST.Y; VIEW_16_3=(int16_t)MPU6050_ACC_LAST.Z; VIEW_16_4=(int16_t)MPU6050_GYRO_LAST.X; VIEW_16_5=(int16_t)MPU6050_GYRO_LAST.Y; VIEW_16_6=(int16_t)MPU6050_GYRO_LAST.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; 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; return; } cnt_a++; } }

/**************************实现函数******************************************** //将iic读取到得数据分拆,放入相应寄存器,更新MPU6050_Last

*******************************************************************************/

void MPU6050_READ(void) { i2cRead(MPU6050_Addr,MPU6050_RA_ACCEL_XOUT_H,14,mpu6050_buffer); }

/**************************实现函数******************************************** *函数原型: u8 IICwriteBit(u8 dev, u8 reg, u8 bitNum, u8 data)

*功 能: 读 修改 写 指定设备 指定寄存器一个字节 中的1个位 输入 dev 目标设备地址 reg 寄存器地址

bitNum 要修改目标字节的bitNum位

data 为0 时，目标位将被清0 否则将被置位 返回 成功 为1 失败为0

*******************************************************************************/

void IICwriteBit(u8 dev, u8 reg, u8 bitNum, u8 data){ u8 b; i2cRead(dev, reg, 1, &b); b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); i2cWrite(dev, reg, b); }

/**************************实现函数******************************************** *函数原型: u8 IICwriteBits(u8 dev,u8 reg,u8 bitStart,u8 length,u8 data) *功 能: 读 修改 写 指定设备 指定寄存器一个字节 中的多个位 输入 dev 目标设备地址 reg 寄存器地址

bitStart 目标字节的起始位 length 位长度

data 存放改变目标字节位的值 返回 成功 为1 失败为0

*******************************************************************************/

void IICwriteBits(u8 dev,u8 reg,u8 bitStart,u8 length,u8 data) { u8 b,mask; i2cRead(dev, reg, 1, &b); mask = (0xFF << (bitStart + 1)) | 0xFF >> ((8 - bitStart) + length - 1); data <<= (8 - length); data >>= (7 - bitStart); b &= mask; b |= data; i2cWrite(dev, reg, b); }

/**************************实现函数******************************************** *函数原型: void MPU6050_setClockSource(uint8_t source) *功 能: 设置 MPU6050 的时钟源 * CLK_SEL | Clock Source

* --------+-------------------------------------- * 0 | Internal oscillator

* 1 | PLL with X Gyro reference * 2 | PLL with Y Gyro reference * 3 | PLL with Z Gyro reference

* 4 | PLL with external 32.768kHz reference * 5 | PLL with external 19.2MHz reference * 6 | Reserved

* 7 | Stops the clock and keeps the timing generator in reset

*******************************************************************************/

void MPU6050_setClockSource(uint8_t source){ IICwriteBits(MPU6050_Addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); }