MPU6050

最新推荐文章于 2024-09-12 16:40:05 发布

南里IA

最新推荐文章于 2024-09-12 16:40:05 发布

阅读量676

点赞数 16

文章标签：其他 stm32

本文链接：https://blog.csdn.net/m0_46503733/article/details/140438087

版权

MPU6050

欧拉角：以运动物体来建立坐标系，通过绕轴旋转表示运动物体当前的状态

欧拉角的样式

Yaw --偏航角绕着Z轴旋转的角度-也就是水平方向上的夹角变化
Roll–翻滚角 --绕着X轴旋转的角度-类似打滚
Pitch–俯仰角 - 绕着Y轴旋转的角度是不是抬头点头这种

加速度传感器

测三个轴向的加速度
http://t.tutu.to/img/mSOs6
由于机器抖动造成严重的噪声出现

陀螺仪与姿态传感器

陀螺仪是用来测量角速度的工具，也就是绕几个轴旋转的角速度–单位是度每秒。

用的是在及其短时间的微分得到的

公式：新的角度 = 上一次的角度 +角速度 *时间

这个会累积误差造成错误

传感器的融合

融合后的结果 = a * 陀螺仪测量结果 + （1-a）*加速度计测量结果

a ：陀螺仪的权重 1-a :加速度计的权重，可以微调

先读取三轴加速度 ax,ay,az,然后通过反正切的公式去求欧拉角 roll以及pitch
再通陀螺仪加速度计算出三轴的角速度 gx,gy,gz
使用原来的角度加上 gx/gy/gz * t(变化时间极短的t)
开始对两组的测量结果进行以融合
加速度计是不可以测量出偏航角Yaw的
Yaw = Yawg
roll = a *rollg + (1-a)*rolla
pitch = a*pitchg * (1-a)*pitcha
这就是得到了融合之后的值

这个a 的取值公式： a = tt / (tt(希腊字母t)+t(变化时间t))

t：读取传感器的时间间隔,可能单位是ms

tt: 0.1s ,一般

带dmp的6轴运动传感器 --DMP --数字运动处理器

芯片上面的XDA引脚以及SCL引脚就是用于将6轴的传感器扩展到9轴（接一个磁力计）.
INT–中断引脚–每当一个Sample的数据采集完成之后就会发送脉冲。

初始化部分

先是复位以及唤醒然后就是后续的这一部分操作
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);//设置采样率函数，这个寄存器的值需要手动计算
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18); //设置加速度计的量程，一共是0-1-2-3 *9.8;第四位和第三位设置其余的直接设置为0
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);//设置陀螺仪的量程；默认2000度每秒

具体的流程---->开始–设备复位-延时100ms-唤醒-设置采样率-设置量程-结束

进程函数部分-此时就是读取，后续还需要组合

void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
						int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
uint8_t DataH, DataL;

DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
*AccX = (DataH << 8) | DataL;

DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
*AccY = (DataH << 8) | DataL;

DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
*AccZ = (DataH << 8) | DataL;

DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
*GyroX = (DataH << 8) | DataL;

DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
*GyroY = (DataH << 8) | DataL;

DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
*GyroZ = (DataH << 8) | DataL;

这里面的值还需要根据上面的转换功公式进行进一步的换算。

加速度计算的结果也需要进行除

温度转换：原始值 /340 +36.53.

角速度的值是根据设置的量程进行更改相应的系数。 /16.4f

f是因为这个可以直接转换成16进制了

读取原始数据

根据采集到的加速度值计算欧拉角

roll = arctan(ay,az)

pitch = -arctan2(ax,az);

c语言代码就是：roll = arctan(ay,az)/3.121593 *180;//弧度转角度

使用陀螺仪计算欧拉角

pitch= pitch(t-tt)+gy*tt;
yaw = yaw(t-tt)+gz*tt;
roll = roll(t-tt)+gx*tt;

新角度 = 上次角度 +角速度*时间,下面是C语言代码

yaw_g = yaw +gz *dt;
roll_g = roll+gx*dt;
pitch = pitch +gy*dt;

dt是自己取时间5ms

后续就是进行一个数据的融合计算

此时其中的参数a 就是根据上面的提示得到了之后，设置的一个参数变量=0.95238；

roll = a *roll_g +(1-a)*roll_a；然后就可以了

然后使用联机软件进行一个实时的调试。

调试部分的代码

#include "stm32f10x.h"                  // Device header
#include "MPU6050.h"
#include "MyI2C.h"
#include "Delay.h"
#include <math.h>
#define MPU6050_AddRESS 0xD0
static float ax,ay,az,gx,gy,gz,yaw1,roll1,pitch1;
//姿态解算部分
//写地址进去
void MPU6050_WriteReg(uint8_t RegAddress,uint8_t Data)
{
	I2C_Start();
	MyI2C_SendByte(MPU6050_AddRESS);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(RegAddress);//将指定寄存器的地址写进去
	MyI2C_ReceiveAck();
	/*如果需要发送多个字节的内容就直接把这个
	在这里加一个循环进行处理，套上这两句代码
	*/
	MyI2C_SendByte(Data);//写入指定寄器的的数据了
	MyI2C_ReceiveAck();
	I2C_Stop();
}
//按照指定地址读一个字节的时序
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Byte;
	I2C_Start();
	MyI2C_SendByte(MPU6050_AddRESS);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(RegAddress);//将指定寄存器的地址写进去
	MyI2C_ReceiveAck();
	
	I2C_Start();
	MyI2C_SendByte( MPU6050_AddRESS |0x01);//此时才是读里面的地址了
	MyI2C_ReceiveAck();
	Byte = MyI2C_ReceiveByte();
	MyI2C_SendAck(1); //1表示非应答，就不再接收数据了
	I2C_Stop();
	return Byte;
}
//此时配置好之后，陀螺仪内部就在连续不断的进行转换了
void MPU6050_Init(void)
{
	MyI2C_Init(); //初始化IIC接口
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x80);//只需要把寄存器的地址以及要写入的值写进去就可以了--此时就是发送复位命令
	Delay_ms(100);//等待复位完成
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);//唤醒这个寄存器
	//MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00); 
	/*将传感器的采样率设置成200hz*/
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x04);
	/*设置加速度计的量程--此时设置成2g*/
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);
	/*设置陀螺仪的量程 ---2000度每秒此时*/
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);//陀螺仪寄存器

}
uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

//获取寄存器中数据的函数
//会返回6个int16_6的数据分别表示加速度的值和陀螺仪的值
//直接传入指针，就可以直接在主函数中的到该变量的值了
void MPU6050_GetData(float *AccX, float *AccY, float *AccZ, 
						float *GyroX, float *GyroY, float *GyroZ,float *yaw,float *roll,float *pitch)
{
	uint8_t DataH, DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); //每一次都是先读取高八位数据，在读取第八位胡数据之后进行封装
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
	*AccX = (int16_t)((DataH << 8) | DataL)/16384.0f;
	ax = *AccX;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
	*AccY = (int16_t)((DataH << 8) | DataL)/16384.0f;
	ay = *AccY;
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
	*AccZ = (int16_t)((DataH << 8) | DataL)/16384.0f;
	az =*AccZ;
	//绕X轴旋转的角速度的值
	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
	*GyroX = (int16_t)((DataH << 8) | DataL)/16.4f;
	gx =*GyroX;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
	*GyroY = (int16_t)((DataH << 8) | DataL)/16.4f;
	gy = *GyroY;
	
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
	*GyroZ =(int16_t)((DataH << 8) | DataL)/16.4f;
	gz = *GyroZ;
	/*使用加速度计计算翻滚角的值*/
	float roll_a = atan2(*AccY,*AccZ)/3.141593f *180.0f;
	/*计算俯仰角的值*/
	float pitch_a = -atan2(*AccX,*AccZ)/3.141593f *180.0f;
	/*使用陀螺仪计算欧拉角*/
	float yaw_g = *yaw + (*GyroZ)*0.005;//偏航角
	float roll_g = *roll + (*GyroX)*0.005;//翻滚角
	float pitch_g = *pitch + (*GyroY)*0.005;//俯仰角
	
	const float alpha =0.95238;
	//进行融合
	*yaw  = yaw_g;
	yaw1 = *yaw;
	*roll = alpha *roll_g+(1-alpha)*roll_a;
	roll1 = *roll;
	*pitch = alpha *pitch_g+(1-alpha)*pitch_a;
	pitch1 = *pitch;
}

//在主函数中存在三个数组，使用这几个变量将各个方向的值进行解算之后，接收到数组里面去，然后就在主函数中得到实现

void MPU6050_GetDa(float *Pa,float *pGro,float *pFan)
{
	Pa[0] = ax;
	Pa[1] = ay;
	Pa[2] = az;
	
	//陀螺仪的值
	pGro[0] =gx;
	pGro[1] =gy;
	pGro[2] =gz;
	
	pFan[0] = yaw1;
	pFan[1] = roll1;
	pFan[2] = pitch1;
}