/************************************************************************** 函数功能:读取MPU6050内置温度传感器数据 入口参数:无 返回 值:摄氏温度 作 者:平衡小车之家 **************************************************************************/ int Read_Temperature(void) { float Temp; Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L); if(Temp>32768) Temp-=65536; Temp=(36.53+Temp/340)*10; return (int)Temp; }
/************************************************************************** 函数功能:获取角度 入口参数:获取角度的算法 1:无 2:卡尔曼 3:互补滤波 返回 值:无 **************************************************************************/ void Get_Angle(u8 way) { float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z; if(way==1) //DMP没有涉及到严格的时序问题,在主函数读取 { } else { Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪 Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪 Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度记 Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度记 if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换 if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换 if(Accel_X>32768) Accel_X-=65536; //数据类型转换 if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换 Gyro_Balance=-Gyro_Y; //更新平衡角速度 Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算与地面的夹角 Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换 if(Way_Angle==2) Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波 else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互补滤波 Angle_Balance=angle; //更新平衡倾角 Gyro_Turn=Gyro_Z; //更新转向角速度 } }
/**************************实现函数******************************************** *函数原型: u8 IICreadByte(u8 dev, u8 reg, u8 *data) *功 能: 读取指定设备 指定寄存器的一个值 输入 dev 目标设备地址 reg 寄存器地址 *data 读出的数据将要存放的地址 返回 1 *******************************************************************************/ u8 IICreadByte(u8 dev, u8 reg, u8 *data){ *data=I2C_ReadOneByte(dev, reg); return 1; }
/**************************实现函数******************************************** *函数原型: void HMC58X3_getID(char id[3]) *功 能: 读取芯片的ID 输入参数: ID存放的数组 输出参数: 无 *******************************************************************************/ void HMC58X3_getID(char id[3]) { id[0]=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDA); id[1]=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDB); id[2]=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDC); } // getID().