一、运动传感器与姿态测量
1、欧拉角
欧拉角:以运动物体建立坐标系，通过绕轴旋转来表示运动物体当前的姿态
(Yaw -偏航角 Roll-翻滚角)
(Pitch -俯仰角)
2、加速度计测量姿态
加速度计，也被称为重力传感器G-Sensor，是一种测量物体加速度的传感器
加速度计通过测量物体在三个轴向上的加速度来推算物体的姿态。在静止或匀速直线运动状态下，加速度计可以准确地测量出重力加速度在三个轴向上的分量，从而推算出物体的俯仰角（Pitch）和横滚角（Roll）。然而，由于加速度计无法直接测量绕垂直轴的旋转（即偏航角Yaw），因此无法单独测量出完整的欧拉角。
优点：能直接算出欧拉角
缺点：外部震动会引入噪声
3、陀螺仪测量姿态
陀螺仪是一种测量角速度的传感器，它可以感知物体绕三个轴的旋转角速度。通过积分陀螺仪输出的角速度信号，可以得到物体的旋转角度，从而推算出欧拉角。然而，陀螺仪的积分过程会导致误差的累积，特别是在长时间运行或受到较大干扰时，误差会显著增加。
优点：准确、无干扰
缺点：误差会累积
4、传感器融合
加速度计和陀螺仪在测量欧拉角时的优缺点：
为了克服加速度计和陀螺仪各自的缺点，提高姿态测量的准确性和稳定性，通常会将两者的数据进行融合。数据融合的方法有多种，包括互补滤波、卡尔曼滤波等。这里利用的是互补滤波的方法，它利用加速度计在静止或匀速直线运动状态下测量准确的优点，以及陀螺仪在动态过程中测量角速度的优点，通过加权平均或叉乘运算将两者的数据进行融合。这种方法可以减小陀螺仪积分过程中的误差累积，同时提高加速度计在动态过程中的响应速度。
二、MPU6050介绍
说明：
DMP（Digital Motion Processor，数字运动处理器）是一种专门用于处理和分析运动数据的集成电路或处理器。DMP通常与传感器（如加速度计、陀螺仪和磁力计）结合使用，以提供精确、实时的姿态和运动信息。
工作原理：DMP通过内置的算法和滤波器对传感器数据进行处理，从而提取出有用的运动信息。这些算法可以包括姿态估计、运动分类、步态分析等。DMP能够高效地处理传感器数据，减少噪声和误差的影响，提供准确的运动参数。
三、MPU6050通信及配置
1、通信方式
IIC通信,可支持快速IIC
2、器件地址
有一个引脚可以产生逻辑电平0 1
如果地址引脚接地 七位器件地址0x68
如果地址引脚接高 七位器件地址0x69
我们板子默认 接地 七位器件地址0x68
注意，要加读写控制位
写： (0x68 << 1) //0xd0
读： (0x68 << 1) | 0x01 //0xd1
3、MUP6050通信时序
1.主控芯片发送一个字节到MPU6050
设置一个寄存器的时序
2.主控芯片发送多个字节到MPU6050
设置地址连续的多个寄存器
3.主控芯片从MPU6050中读一个字节回来
读某一个寄存器里面的值
4.主控芯片从MPU6050中连续读多个字节回来
读地址连续的多个寄存器的值，比如读xyz方向的值
四、MPU6050相关寄存器
MPU6050初始化就是把上面的寄存器配置
五、MPU6050获取陀螺仪和加速度数据
初始化后,发送数据寄存器地址,就可以读到数据寄存器的内容,将两个8位数据合成16位数据
带入到公式中就可以得到具体角度值和加速度值
说明:
数据寄存器因为地址连续,所以可以连续读出
六、MPU6050程序设计
①IIC通信相关程序
/***********************************************
*函数名 :iic_IO_init
*函数功能 :IIC所用IO口初始化配置
*函数参数 :无
*函数返回值:无
*函数描述 :SCL----PB8
SDA----PB9
************************************************/
void iic_IO_init(void)
{
//端口时钟使能
RCC->AHB1ENR |= (1<<1);
//端口模式配置
GPIOB->MODER &= ~((3<<16)|(3<<18));
GPIOB->MODER |= ((1<<16)|(1<<18));
//输出类型
GPIOB->OTYPER &= ~(1<<8); //时钟线配置位推挽输出
GPIOB->OTYPER |= (1<<9); //数据线配置为开漏输出
//输出速度
GPIOB->OSPEEDR &= ~((3<<16)|(3<<18));
//上下拉
GPIOB->PUPDR &= ~((3<<16)|(3<<18));
//输出数据寄存器让时钟线和数据线处于空闲转态
GPIOB->ODR |= ((1<<8) | (1<<9));
}
/***********************************************
*函数名 :iic_star
*函数功能 :IIC起始信号函数
*函数参数 :无
*函数返回值:无
*函数描述 :
************************************************/
void iic_star(void)
{
//时钟线拉低 可以动数据线
IIC_SCL_L;
//数据线拉高 保证可以出现下降沿
IIC_SDA_OUT_H;
//时钟线拉高 起始信号的条件
IIC_SCL_H;
TIM11_delay_us(5);
//数据线拉低 产生了起始信号
IIC_SDA_OUT_L;
TIM11_delay_us(5);
IIC_SCL_L;//安全作用
}
/***********************************************
*函数名 :iic_stop
*函数功能 :IIC停止信号函数
*函数参数 :无
*函数返回值:无
*函数描述 :
************************************************/
void iic_stop(void)
{
IIC_SCL_L;
IIC_SDA_OUT_L;
IIC_SCL_H;
TIM11_delay_us(5);
IIC_SDA_OUT_H;
TIM11_delay_us(5);
}
/***********************************************
*函数名 :iic_send_ack
*函数功能 :IIC发送应答/不应答信号函数
*函数参数 :u8 ack
*函数返回值:无
*函数描述 :ack 传入 0 表示发送应答
ack 传入 1 表示发送不应答
************************************************/
void iic_send_ack(u8 ack)
{
IIC_SCL_L;
TIM11_delay_us(3);
if(ack == 0) IIC_SDA_OUT_L;
else IIC_SDA_OUT_H;
TIM11_delay_us(2);
IIC_SCL_H;
TIM11_delay_us(5);
IIC_SCL_L;//安全作用
}
/***********************************************
*函数名 :iic_get_ack
*函数功能 :IIC检测应答/不应答信号函数
*函数参数 :无
*函数返回值:u8
*函数描述 :0 应答信号
1 不应答信号
************************************************/
u8 iic_get_ack(void)
{
u8 ack;
/*将数据线切换位输入模式*/
IIC_SCL_L;
IIC_SDA_OUT_H;
/*检测应答*/
IIC_SCL_L; //主机帮助从机把时钟线拉低，从机才可以自动改变数据线
TIM11_delay_us(5);
IIC_SCL_H; //主机拉高时钟线才可以读数据线
TIM11_delay_us(5);
if(IIC_SDA_IN) ack = 1; //不应答信号
else ack = 0; //应答信号
IIC_SCL_L; //安全作用
return ack;
}
/***********************************************
*函数名 :iic_send_byte
*函数功能 :IIC发送一个字节函数
*函数参数 :u8 data
*函数返回值:无
*函数描述 :
************************************************/
void iic_send_byte(u8 data)
{
u8 i;
for(i=0;i<8;i++)
{
IIC_SCL_L;
TIM11_delay_us(3);
if(data & 0x80)
{
IIC_SDA_OUT_H;
}
else
{
IIC_SDA_OUT_L;
}
TIM11_delay_us(2);
IIC_SCL_H; //帮助从机拉高时钟线，从机才可以读这一位数据
TIM11_delay_us(5);
//下一位数据
data = data << 1;
}
IIC_SCL_L; //安全作用
}
/***********************************************
*函数名 :iic_rec_byte
*函数功能 :IIC接收一个字节函数
*函数参数 :无
*函数返回值:u8
*函数描述 :
************************************************/
u8 iic_rec_byte(void)
{
u8 i;
u8 data;
/*IO切换为输入模式*/
IIC_SCL_L;
IIC_SDA_OUT_H;
/*接收一个字节数据*/
for(i=0;i<8;i++)
{
IIC_SCL_L; //帮助从机拉低时钟线，从才会改变数据线
TIM11_delay_us(5);
IIC_SCL_H; //主机可以读数据线
TIM11_delay_us(5);
data = data << 1;
if(IIC_SDA_IN)
{
data |= 0x01;
}
}
IIC_SCL_L; //安全作用
return data;
}
②MPU6050相关程序
主控芯片发送多个字节到MPU6050函数
/***********************************************
*函数名 ：mpu6050_Write
*函数功能 ：写多个字节数据到MPU6050
*函数参数 ：u8 reg, u8 len, u8* data
*函数返回值：无
*函数描述 ：
***********************************************/
void mpu6050_Write(u8 reg,u8 len,u8* data)
{
u8 i;
iic_star();
iic_send_byte(0x68<<1); //器件地址+写控制位
iic_get_ack();
iic_send_byte(reg); //发送寄存器地址
iic_get_ack();
//连续发送数据
for (i = 0; i < len; i++)
{
iic_send_byte(data[i]);
iic_get_ack();
}
iic_stop();
}
主控芯片从MPU6050中连续读多个字节回来函数
/***********************************************
*函数名 ：mup6050_Read
*函数功能 ：从MPU6050读多个字节数据
*函数参数 ：u8 reg,u8 len, u8* buf
*函数返回值：无
*函数描述 ：
***********************************************/
void mup6050_Read(u8 reg,u8 len, u8* buf)
{
iic_star();
iic_send_byte(0x68<<1);
iic_get_ack();
iic_send_byte(reg); //寄存器的起始地址
iic_get_ack();
iic_star();
iic_send_byte(0x68<<1 | 0x01);
iic_get_ack();
while(1)
{
*buf = iic_rec_byte();
len--;
buf++;
if(len == 0)
{
iic_send_ack(1);
break;
}
else
{
iic_send_ack(0);
}
}
iic_stop();
}
MPU6050初始化配置函数
/***********************************************
*函数名 ：MPU6050_init
*函数功能 ：MPU6050_init 初始化配置
*函数参数 ：无
*函数返回值：无
*函数描述 ：
***********************************************/
void MPU6050_init(void)
{
u8 re_buff[] = {0x89,0x09,0x18,0x00,0x04,0x13}; //采样频率计算值19 十六进制0x13
/*IIC初始化*/
iic_IO_init();
/*MPU6050*/
mpu6050_Write(PWR_MGMT_1,1,&re_buff[0]); //复位MPU6050
tim11_delay_ms(100); //不放延时会出问题
mpu6050_Write(PWR_MGMT_1, 1,&re_buff[1]); //解除休眠状态
mpu6050_Write(GYRO_CONFIG, 1,&re_buff[2]); //陀螺仪范围 2000 16.4
mpu6050_Write(ACCEL_CONFIG, 1,&re_buff[3]); //2g 16384
mpu6050_Write(CONFIG, 1,&re_buff[4]); //20带宽 1KHZ
mpu6050_Write(SMPLRT_DIV, 1,&re_buff[5]); //采样频率19
}
获取XYZ方向数据函数
/***********************************************
*函数名 ：DataSynthesis
*函数功能 ：获取某个方向的高低8位数据并且合成16位数据
*函数参数 ：寄存器地址
*函数返回值：short
*函数描述 ：
***********************************************/
short DataSynthesis(u8 REG_Address)
{
u8 uByte[2];
mup6050_Read(REG_Address,2,uByte) ;
return ((uByte[0] << 8) | uByte[1]); /*返回合成数据*/
}
/***********************************************
*函数名 ：ReadAcceData
*函数功能 ：读加速度和陀螺仪三个方向的数据
*函数参数 ：无
*函数返回值：ACCG_t
*函数描述 ：
***********************************************/
ACCG_t ReadAcceData(void)
{
ACCG_t ag;
//读加速度
ag.x_Axis = DataSynthesis(ACCEL_XOUT_H) / 16384.0 ;
ag.y_Axis = DataSynthesis(ACCEL_YOUT_H) / 16384.0;
ag.z_Axis = DataSynthesis(ACCEL_ZOUT_H) / 16384.0 ;
//陀螺仪
ag.x_Gxis = DataSynthesis(GYRO_XOUT_H) / 16.4;
ag.y_Gxis = DataSynthesis(GYRO_YOUT_H) / 16.4;
ag.z_Gxis = DataSynthesis(GYRO_ZOUT_H) / 16.4;
return ag;
}