读MPU6050数据的文件
#ifndef __MPU6050_H__
#define __MPU6050_H__
sbitSCL=P2^5 //IIC时钟引脚定义
sbitSDA=P2^6 //IIC数据引脚定义
void InitMPU6050() //初始化MPU6050
void Delay5us()
void I2C_Start()
void I2C_Stop()
void I2C_SendACK(bit ack)
bit I2C_RecvACK()
void I2C_SendByte(unsigned char dat)
unsigned char I2C_RecvByte()
void I2C_ReadPage()
void I2C_WritePage()
unsigned char Single_ReadI2C(unsigned char REG_Address) //读取I2C数据
void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data) //向I2C写入数据
// 定义MPU6050内部地址
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
void Delay5us() //误差 -0.083333333333us
{
unsigned char a
for(a=27a>0a--)
}
void I2C_Start()
{
SDA = 1 //拉高数据线
SCL = 1 //拉高时钟线
Delay5us()//延时
SDA = 0 //产生下降沿
Delay5us()//延时
SCL = 0 //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0 //拉低数据线
SCL = 1 //拉高时钟线
Delay5us()//延时
SDA = 1 //产生上升沿
Delay5us()//延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack //写应答信号
SCL = 1 //拉高时钟线
Delay5us()//延时
SCL = 0 //拉低时钟线
Delay5us()//延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1 //拉高时钟线
Delay5us()//延时
CY = SDA //读应答信号
SCL = 0 //拉低时钟线
Delay5us()//延时
return CY
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(unsigned char dat)
{
unsigned char i
for (i=0i<8i++) //8位计数器
{
dat <<= 1 //移出数据的最高位
SDA = CY //送数据口
SCL = 1 //拉高时钟线
Delay5us()//延时
SCL = 0 //拉低时钟线
Delay5us()//延时
}
I2C_RecvACK()
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
unsigned char I2C_RecvByte()
{
unsigned char i
unsigned char dat = 0
SDA = 1 //使能内部上拉,准备读取数据,
for (i=0i<8i++) //8位计数器
{
dat <<= 1
SCL = 1 //拉高时钟线
Delay5us()//延时
dat |= SDA//读数据
SCL = 0 //拉低时钟线
Delay5us()//延时
}
return dat
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data)
{
I2C_Start() //起始信号
I2C_SendByte(SlaveAddress) //发送设备地址+写信号
I2C_SendByte(REG_Address) //内部寄存器地址，
I2C_SendByte(REG_data) //内部寄存器数据，
I2C_Stop() //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
unsigned char Single_ReadI2C(unsigned char REG_Address)
{
unsigned char REG_data
I2C_Start() //起始信号
I2C_SendByte(SlaveAddress) //发送设备地址+写信号
I2C_SendByte(REG_Address)//发送存储单元地址，从0开始
I2C_Start() //起始信号
I2C_SendByte(SlaveAddress+1) //发送设备地址+读信号
REG_data=I2C_RecvByte() //读出寄存器数据
I2C_SendACK(1) //接收应答信号
I2C_Stop() //停止信号
return REG_data
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00) //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07)
Single_WriteI2C(CONFIG, 0x06)
Single_WriteI2C(GYRO_CONFIG, 0x18)
Single_WriteI2C(ACCEL_CONFIG, 0x01)
}
//**************************************
//合成数据
//**************************************
int GetData(unsigned char REG_Address)
{
char H,L
H=Single_ReadI2C(REG_Address)
L=Single_ReadI2C(REG_Address+1)
return (H<<8)+L //合成数据
}
#endif
下面是滤波
#ifndef __Kalman_H__
#define __Kalman_H__
void Kalman_Filter(float Accel,float Gyro)
void Angle_Calcu(void)
typedef unsigned char uchar
typedef unsigned short ushort
typedef unsigned int uint
//******角度参数************
float Gyro_y //Y轴陀螺仪数据暂存
float Angle_gy //由角速度计算的倾斜角度
float Accel_x //X轴加速度值暂存
float Angle_ax //由加速度计算的倾斜角度
float Angle//小车最终倾斜角度
uchar value
//******卡尔曼参数************
float Q_angle=0.001
float Q_gyro=0.003
float R_angle=0.5
float dt=0.01 //dt为kalman滤波器采样时间
char C_0 = 1
float Q_bias, Angle_err
float xdata PCt_0, PCt_1, E
float xdata K_0, K_1, t_0, t_1
float xdata Pdot[4] ={0,0,0,0}
float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } }
// 卡尔曼滤波
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt//先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]// Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1]
Pdot[2]=- PP[1][1]
Pdot[3]=Q_gyro
PP[0][0] += Pdot[0] * dt // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt
PP[1][1] += Pdot[3] * dt
Angle_err = Accel - Angle//zk-先验估计
PCt_0 = C_0 * PP[0][0]
PCt_1 = C_0 * PP[1][0]
E = R_angle + C_0 * PCt_0 K_0 = PCt_0 / E
K_1 = PCt_1 / E
t_0 = PCt_0
t_1 = C_0 * PP[0][1]
PP[0][0] -= K_0 * t_0 //后验估计误差协方差
PP[0][1] -= K_0 * t_1
PP[1][0] -= K_1 * t_0
PP[1][1] -= K_1 * t_1
Angle += K_0 * Angle_err //后验估计
Q_bias += K_1 * Angle_err //后验估计
Gyro_y = Gyro - Q_bias //输出值(后验估计)的微分=角速度
}
//*********************************************************
// 倾角计算卡尔曼融合
//在程序中利用Angle+=(Gyro - Q_bias) * dt计算出陀螺仪积分出的角度，其中Q_bias是陀螺仪偏差。
//此时利用陀螺仪积分求出的Angle相当于系统的估计值，得到系统的观测方程；而加速度计检测的角度Accel相当于系统中的测量值，得到系统状态方程。
//程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度。根据Pdot = A*P + P*A' + Q_angle计算出先验估计协方差的微分，用于将当前估计值进行线性化处理。其中A为雅克比矩阵。
//随后计算系统预测角度的协方差矩阵P。计算估计值Accel与预测值Angle间的误差Angle_err。
//计算卡尔曼增益K_0,K_1，K_0用于最优估计值，K_1用于计算最优估计值的偏差并更新协方差矩阵P。
//通过卡尔曼增益计算出最优估计值Angle及预测值偏差Q_bias，此时得到最优角度值Angle及角速度值。
void Angle_Calcu(void)
{
//------加速度--------------------------
//范围为2g时换算关系16384 LSB/g
//角度较小时x=sinx得到角度弧度, deg = rad*180/3.14
//因为x>=sinx,故乘以1.3适当放大
Accel_x = GetData(ACCEL_XOUT_H) //读取X轴加速度
Angle_ax = (Accel_x - 1100) /16384 //去除零点偏移,计算得到角度弧度
Angle_ax = Angle_ax*1.2*180/3.14//弧度转换为度,
//-------角速度-------------------------
//范围为2000deg/s时换算关系16.4 LSB/(deg/s)
Gyro_y = GetData(GYRO_YOUT_H) //静止时角速度Y轴输出为-30左右
Gyro_y = -(Gyro_y + 30)/16.4//去除零点偏移计算角速度值,负号为方向处理
//Angle_gy = Angle_gy + Gyro_y*0.01 //角速度积分得到倾斜角度.
//-------卡尔曼滤波融合-----------------------
Kalman_Filter(Angle_ax,Gyro_y) //卡尔曼滤波计算倾角
/*//-------互补滤波-----------------------
//补偿原理是取当前倾角和加速度获得倾角差值进行放大然后与
//陀螺仪角速度叠加后再积分从而使倾角最跟踪为加速度获得的角度
//0.5为放大倍数可调节补偿度0.01为系统周期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01)*/
}
#endif
把Angle显示出来就行了
论坛上对mpu6050的资料和讨论并不多，很多坛友都说驱动失败，老是显示0.以下就谈谈我的一些血与泪的教训：
昨天开始接触mpu6050，在网上查了很多资料，下载程序，准备一展身手。首先看了mpu6050中文资料，之后又看了那个mpu6050的测试程序，把这些看明白之后就开始写程序了。我不是直接把程序复制过去，只是复制mpu6050的地址和初始化，IIC并没有复制，就复制我上次写的24C02的那个程序，想不到，这给了我血与泪的教训，我原来是直接把IIC复制过来的，并没有多留意。之后初始化mpu6050，写入地址，读出数据，下载到单片机之后，LCD上显示000001，我感到郁闷，之后又调试，以为是显示不对，又写显示，之后又下载，结果还是老样，这样半天就过去了。驱动没成功，又怀疑芯片或引脚有问题，继续调试，也没成功。就一一对应地看了地址，又看了初始化，发现没错，调试还是不成功。最后干脆不接IIC总线，竟然发现了个天大的秘密，接不接IIC，LCD都显示000001，我又用示波器测试波形，发现波形正确。在网上又查了别人的资料，在论坛上也很少有关于mpu6050的资料，也看了比别人的一些讨论。很多坛友都说驱动失败，老是显示0.
没办法，一天就这样过去了，今天早上，我又仔细看了程序，出乎我的想象，竟然是IIC的那个地址没改，原来写24C02的那个地址是a0，还是原封不动，把我吓了一跳。把这些改过来之后，一切正常，能显示加速度和陀螺仪。血与泪的教训啊，是自己不细心造成的，忘记改地址！今天早上竟然花了不到2个钟就调出来了，惊喜之时就写了这个分享，希望对大家有用。
还没经过软件滤波
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）