STM32--MPU6050 DMP读角度总结

Posted QWQ_DIODA

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了STM32--MPU6050 DMP读角度总结相关的知识,希望对你有一定的参考价值。

前言

我TM爆肝!!


MPU6050简介

在这里插入图片描述
MPU6050一般为六轴或九轴传感器

正常情况下为:

  • 三轴陀螺仪(测角速度)
  • 三轴加速度仪
  • 三轴磁场轴(有些没有)

一般测量角度,陀螺仪和加速度仪就够了

若想测量角度,通过一些滤波算法,可以将所测速度和所测加速度转换成角度数据。

但是MPU6050自带DMP自结算,可将角度数据结算成为四元角,我们只需要移植并调用DMP库函数就行。

1.1、引脚说明

在这里插入图片描述
一般为六个引脚

  • SDA和SCL为IIC通讯引脚
  • VCC、GND不需要多讲
  • 当获得数据INT引脚可触发EXIT中断,再中断中可以获得姿态角
  • AD0为IIC地址引脚,接GND和悬空地址为0X68,接VCC地址为0X69

DMP库简介

MPU6050自带的解算角度数据的dmp库。
结算数据速度最大可以达到200HZ。
根据我们上次讲的增量式PID教程
200hz频率的数据获取完全可以够用了。

解算得出的数据为四元角数据

MPU6050初始化函数讲解(若使用DMP库则可以忽略)

MPU6050使用的是IIC通讯,由于STM32硬件IIC有bug,所以我们使用软件模拟IIC。
模拟IIC的代码就略了。
这里粘贴原子得代码。

u8 MPU_Init(void)
{ 
	u8 res; 
	MPU_IIC_Init();//初始化IIC总线
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
    delay_ms(100);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050 
	MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
	MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
	MPU_Set_Rate(50);						//设置采样率50Hz
	
	MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
	res=MPU_Read_Byte(MPU_DEVICE_ID_REG); 
	if(res==MPU_ADDR)//器件ID正确
	{
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
		MPU_Set_Rate(50);						//设置采样率为50Hz
 	}else return 1;
	return 0;
}

这其中配置了

  • 0x6B 电源管理寄存器1
  • 0x6C 电源管理寄存器2
  • 0x1B 陀螺仪设置寄存器
  • 0x1C 加速度仪设置寄存器
  • 0x19 陀螺仪分频采样寄存器
  • 0x1A 配置寄存器
  • 0X38 中断使能寄存器
  • 0X6A 用户控制寄存器
  • 0x23 FIFO使能寄存器
  • 0X37 中断/旁路设置寄存器
  • 0X75 器件地址寄存器

查找技术手册,对这些寄存器分析

1、电源管理寄存器1
在这里插入图片描述

  • BIT7:复位:1复位,复位结束后MPU硬件自动清零。
  • BIT6:1为睡眠模式(低功耗),0为正常工作模式,所以需要置零
  • BIT3:是否使能温度传感器,设置0使能。
  • BIT[2~0]:设置时钟设置001使用X轴为参考。在这里插入图片描述
    2、电源管理寄存器2
    在这里插入图片描述
  • BIT[7-6]:控制低功耗得唤醒频率,暂且用不到罢
  • BIT[5-0]:控制陀螺仪和加速度仪xyz轴是进入待机模式,全部置0

3、陀螺仪配置寄存器
在这里插入图片描述
BIT[4-3]:设置陀螺仪量程。

  • 0,±250°/S;
  • 1,±500° /S;
  • 2,±1000° /S;
  • 3,±2000° /S
    陀螺仪ADC分辨率为16位,我们设置为3
    灵敏度为:65536/4000=16.4LSB/(° /S)

4、加速度仪配置寄存器
在这里插入图片描述
BIT[4-3]:

  • 0,± 2g;
  • 1,± 4g;
  • 2,± 8g;
  • 3,± 16g;
    加速度仪的ADC分辨率为16位,我们设置为0
    灵敏度为:65536/4=16384LSB/g

5、陀螺仪分频采样寄存器
在这里插入图片描述
BIT[7-0]:设置采样频率
公式为:采样频率 = 陀螺仪输出频率 /(1+BIT[7-0])
这个采样频率的设置与配置寄存器的设置有关,我们先假定50HZ,陀螺仪频率为1Khz,那么
BIT[7-0] = 1000/50-1=19。

6、配置寄存器
在这里插入图片描述
BIT[2-0]:
在这里插入图片描述
带宽一般为采样频率得一半(你问我为啥,我也不知道,记住就完了)
所以取近似值20
BIT[2-0]就设置为100;

7、中断使能寄存器
在这里插入图片描述
BIT[7-0]全设置为0,关闭所有中断!!!;

8、用户控制寄存器
在这里插入图片描述
BIT5:置零,I2C主模式关闭

9、FIFO使能寄存器
在这里插入图片描述
全部置零,关闭FIFO寄存器!!

10、中断/旁路设置寄存器
在这里插入图片描述

  • BIT7:0时逻辑电平为低,1时逻辑电平为高
  • BIT6:0 INT引脚推挽,1 INT引脚OD
  • BIT5:0 INT引脚产生50微秒脉冲
  • BIT4:0 读取 INT_STATUS (寄存器 58)中断状态位清除。1,任何读取中断状态位清除
    后四位略
    所以设置0x80即低电平有效

11、WHO AM I
在这里插入图片描述
读取该寄存器,获取设备地址,检查IIC和元器件是否有问题。

DMP库移植

在这里插入图片描述
将这里面得六个文件复制粘贴至STM32得工程下
c文件中,需要修改得步骤已经有了
在这里插入图片描述
我们需要创建一个和注释中函数格式完全一致得函数来替代。
替代函数如下

/*********************************

	功能:写数据到MPU6050寄存器
	形参:reg_add为MPU6050寄存器地址,
				dat为MPU6050写入寄存器数据
	返回:无

**********************************/
uint8_t MPU6050_WriteReg(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char const *data)
{
	uint8_t i;
	IIC_Start();
	IIC_Send_Byte((slave_addr<<1)|0);
	if(IIC_Wait_Ack())
	{
		IIC_Stop();
		return 3;
	}
	IIC_Send_Byte(reg_addr);
	IIC_Wait_Ack();
	for(i=0;i<length;i++)
	{
		IIC_Send_Byte(data[i]);
		if(IIC_Wait_Ack())
		{
			IIC_Stop();
			return 1;
		}
	}
	IIC_Stop();
	
	return 0;
}

/*********************************

	功能:从MPU6050数据寄存器中读数据
	形参:reg_add为MPU6050数据寄存器地址,
					*dat 为MPU6050读取所得数据
					num	 为数据长度
	返回:无

**********************************/
uint8_t MPU6050_ReadData(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char *data)
{
	IIC_Start();
	IIC_Send_Byte((slave_addr<<1)|0);
	if(IIC_Wait_Ack())
	{
		IIC_Stop();
		return 1;
	}
	IIC_Send_Byte(reg_addr);
	IIC_Wait_Ack();
	IIC_Stop();
	
	IIC_Start();
	IIC_Send_Byte((slave_addr<<1)+1);
	IIC_Wait_Ack();
	
	while(length){
		if(length == 1)
			*data=IIC_Read_Byte(0);
		else
			*data=IIC_Read_Byte(1);
		length--;
		data++;
	}
	IIC_Stop();
	return 0;
}

/*********************************

	功能:获取时间戳
	形参:*count传递数据
	返回:无

**********************************/
void stm32_get_ms(unsigned long *count)
{
	*count = HAL_GetTick();
}

/*********************************

	功能:延迟时间
	形参:num_ms表示ms时间
	返回:无

**********************************/
void stm32_Delay_ms(unsigned long num_ms)
{
	HAL_Delay(num_ms);
}

修改文件
inv_mpu.c

在这里插入图片描述
修改为
在这里插入图片描述
并将包含文件改变成自己得包含文件

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "spi.h"
#include "main.h"

inv_mpu_dmp_motion_drive.c中同理
在这里插入图片描述
同时,大部分教程中都没写的部分,在DMP驱动文件中,还有一个motion_driver_test.c的文件!!!!
里面也有重要的函数!!很多教程都是直接跳过了,导致我看了好长时间也不懂。
这个文件里有方向转换的函数,自检函数,还有可以学习参考的主函数!!!
我们需要将此文件中的
在这里插入图片描述
在这里插入图片描述
这两个函数复制粘贴进我们的工程中,并使inv_mpu_dmp_motion_drive.c和inv_mpu.c调用它!!!

如果此时移植完编译仍不通过,将错误行注释掉就行!!

DMP库初始化讲解

/**************************************************

	功能:DMP初始化
	形参:无
返回值:0:成功
		 其它:失败

***************************************************/
uint8_t DMP_Init(void)
{
	uint8_t res = 0;
	uint8_t RES = 0;
	RES = mpu_init();
	if(RES == 0)
	{
		res = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
		if(res)return 88;
		res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
		if(res)return 2; 
		res=mpu_set_sample_rate(100);	//设置采样率
		if(res)return 3; 
		res=dmp_load_motion_driver_firmware();		//加载dmp固件
		if(res)return 4; 
		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
		if(res)return 5; 
		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|DMP_FEATURE_android_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
		
		if(res)return 6; 
		res=dmp_set_fifo_rate(100);	//设置DMP输出速率(最大不超过200Hz)
		if(res)return 7; 
		run_self_test();
		res=mpu_set_dmp_state(1);	//使能DMP
		if(res)return 9; 
	}
	else
		return RES;
	for(uint16_t rd=0; rd<300; rd++)				
		{
			//读取MPU内置DMP的姿态
			read_dmp(&pitch,&roll,&yaw);
			HAL_Delay(20);
		}
	return 0;
}

DMP库中存在mpu_init();函数,因此,先前得初始化可以注释掉了(前面的只是为了熟悉或者读取生数据所需)
首先是初始化MPU

让后另一个重点是
在这里插入图片描述

保证读取没有问题。

最后就是四元角的换算

/**************************************************

	功能:DMP读取
	形参:传递读取数据
返回值:0:成功
		 其它:失败

***************************************************/
uint8_t read_dmp(float *pitch, float *roll, float *yaw)
{
	uint8_t more;
	long quat[4];				//四元数
	int16_t gyro[3], accel[3], sensors;
	unsigned long sensor_timestamp;//传感器时间戳
	
	float q0 = 1.0f,q1 = 0.0f,q2 = 0.0f,q3 = 0.0f;
	
	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more))
		return 1;
	if(sensors & 0x100)
	{
		q0 = quat[0]/q30;
		q1 = quat[1]/q30;
		q2 = quat[2]/q30;
		q3 = quat[3]/q30;
		
		*pitch = asin(-2*q1*q3 + 2*q0*q2)* 57.3f;																	//四元数解算, 俯仰角(Pitch)-->绕着X轴旋转
		*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3f;	//四元数解算, 翻滚角(Roll)-->绕着Y轴旋转
		*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3f;								//四元数解算, 偏航角(Yaw)-->绕着Z轴旋转
		
		return 0;
	}
	
	else
		return 2;
}

读取数据排错检查

初始化函数中定义res
用串口发送res的数值来检查程序哪一步出错

常见错误
1、FIFO没开,再初始化前先用寄存器打开FIFO
2、mpu多次初始化
3、移植问题,有些警告没有注释
4、芯片损坏

以上是关于STM32--MPU6050 DMP读角度总结的主要内容,如果未能解决你的问题,请参考以下文章

STM32CubeMX(05) 移植陀螺仪MPU6050的DMP库读取三轴角度,加速度

STM32CubeMX移植MPU6050的DMP库读取角度信息

STM32+MPU6050设计便携式Mini桌面时钟(自动调整时间显示方向)

STM32+MPU6050设计便携式Mini桌面时钟(自动调整时间显示方向)

STM32--MPU内存保护单元

STM32--MPU内存保护单元