/**************************************************************************
函数功能：读取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().