一.MPU6050介绍

1.MPU6050与陀螺仪、加速度计的关系:

MPU6050是InvenSense公司推出的一款全球首款的整合性9轴运动处理传感器,其最大的特色就是:消除了陀螺仪和加速度计的误差,将陀螺仪和加速度计组合在一起,而且缩小了空间。关于陀螺仪、加速度计和MPU6050之间的关系之前在一篇博客中已经做了介绍:博客链接

2.整体概括

MPU6050内部整合了三轴MEMS陀螺仪、三轴MEMS加速度计以及一个可扩展的数字运动处理器DMP,而且还可以连接一个第三方数字传感器(比如:磁力计),这样的话,就可以通过IIC接口输出一个9轴信号。
更加方便的是,有了DMP,可以结合InvenSense公司提供的运动处理资料库,实现姿态解算。通过自带的DMP,可以通过IIC接口输出9轴融合演算的数据,大大降低了运动处理运算对操作系统的负荷,同时也降低了开发难度。

特点:
以数字形式输出 6 轴或 9 轴(需外接磁传感器)的旋转矩阵、四元数(quaternion)、欧
拉角格式(Euler Angle forma)的融合演算数据(需 DMP 支持)

② 具有 131 LSBs/° /sec 敏感度与全格感测范围为±250、±500、±1000 与±2000° /sec
的 3 轴角速度感测器(陀螺仪)
③ 集成可程序控制,范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
④ 移除加速器与陀螺仪轴间敏感度,降低设定给予的影响与感测器的飘移
自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算
数据、感测器同步化、姿势感应等的负荷

内建运作时间偏差与磁力感测器校正演算技术,免除了客户须另外进行校正的需求
⑦ 自带一个数字温度传感器
⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS
⑨ 可程序控制的中断(interrupt),支持姿势识别、摇摄、画面放大缩小、滚动、快速下降
中断、 high-G 中断、零动作感应、触击感应、摇动感应功能
⑩ VDD 供电电压为 2.5V±5%、 3.0V±5%、 3.3V±5%; VLOGIC 可低至 1.8V± 5%
⑪ 陀螺仪工作电流: 5mA,陀螺仪待机电流: 5uA; 加速器工作电流: 500uA,加速器省
电模式电流: 40uA@10Hz
自带 1024 字节 FIFO,有助于降低系统功耗
⑬ 高达 400Khz 的 IIC 通信接口
⑭ 超小封装尺寸: 4x4x0.9mm(QFN)

其检测轴如图所示:

3.引脚说明


如图,MPU6050一共有8个引脚,实际上输出六轴数据时,只用了5个:VCC、GND、SCL、SDA、AD0。下面介绍一下引脚:

  • VCC:供电,3.3V即可
  • GND:接地
  • SCL:连接MCU的IIC时钟接口
  • SAD:连接MCU的IIC数据接口
  • XCL:连接外部设备的IIC时钟接口
  • XAD:连接外部设备的IIC数据接口
  • AD0:地址控制引脚(控制地址的最低位)
  • INT:中断触发接口(不用)

XCL、XDA只有在连接外部设备(比如磁力计的时候才用),AD0用来控制MPU6050的地址,如果AD0低电平,地址就是0X68;如果AD0高电平,地址就是0X69。

4.基本配置及相关寄存器

MCU与MPU6050的通信是建立在IIC通信机制上的,在IIC的基础上,可以实现对MPU6050的寄存器的操作,而MPU6050的运作就是过对寄存器进行读写。所以,了解相关的寄存器和对寄存器的操作是很有必要的。MPU6050的寄存器相关资料都可以在数据手册中查到,下面介绍一下几个重要的寄存器:

电源管理寄存器1

地址:0X68
主要位的功能:

  • DEVICE_RESET:控制复位,1表示复位,复位后会自动清零;
  • SLEEP:控制MPU6050工作模式,1表示睡眠模式,0表示正常工作模式,复位后改位为1,要手动将改位清零;
  • TEMP_DIS:使能温度传感器位,0代表使能;
  • CLKSEL[2:0]:选择系统时钟源,一般采用PLL_X轴陀螺作为参考,具体的时钟源选择及相应位的值如图:

陀螺仪配置寄存器

地址:0X1B

主要位的功能:

  • FS_SEL[1:0]:设置陀螺仪满量程范围:为0代表±250° /S、为1代表±500° /S、为2代表±1000° /S为3代表±2000° /S;

陀螺仪的分辨率是16位,所以在最大量程下灵敏度为: 65536/4000=16.4LSB/(° /S)。

加速度计配置寄存器

地址:0X1C

主要位的功能:

  • AFS_SEL[1:0]:设置加速度计满量程范围:为 0代表±2g、为1代表±4g、为 2代表±8g、为 3代表±16g;

FIFO使能寄存器

地址:0X23

用来控制FIFO功能,相应位对应着相应的传感器FIFO功能,为0代表禁止,为1代表使能。注意:加速度传感器的三个轴的FIFO功能由一个位ACCEL_FIFO_EN控制。在简单读取传感器数据的情况下可以不使用FIFO。

陀螺仪采样率分频寄存器

地址:0X19

改寄存器用来设置MPU6050陀螺仪的采样频率,与之相关的是陀螺仪的输出频率,俩者关系是:采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
陀螺仪输出频率与数字低通滤波器(DLPF)有关,DLPF滤波频率一般设置为采样率的一半。

温度传感器寄存器

地址:高八位0X41、低八位0X42
直接通过读取寄存器中的值来得到温度数据,温度换算公式为:
Temperature = 36.53 + regval/340

二.代码详解

1.框架

我是用STM32驱动MPU6050,MPU6050输出原始的六轴数据,经过DMP处理(有库)得到四元数,再由四元数算出欧拉角:yaw、roll、pitch。由串口打印在电脑屏幕上。

  • 首先,要做底层的IIC驱动,用来和MPU6050建立通信,我在mpu_iic.c中实现了;
  • 然后,有了底层的驱动,就要写一些函数来与MPU6050交流了(通过读写寄存器),还可以写入命令、配置MPU6050、读取原始数据等等,这些操作我都写在mpu6050.c中,当然在mpu6050.h头文件中还包含了MPU各寄存器地址和相关指令。
  • 通过mpu6050.c的实现,就可以读出原始六轴数据,下一步就是通过DMP将原始数据转换为四元数,这一步的DMP算法我水平有限,只能移植InvenSense公司提供的例程。关于移植DMP算法,由于DMP算法本质也是对MPU6050的操作,所以我们只需要向移植过来的算法提供:对MPU6050寄存器执行读和写的函数接口即可,最后通过移植过来的函数直接读出四元数!
  • 然后,就是将四元数转换为欧拉角了,这个比较简单,一个函数就可以实现。
  • 最后,打印串口到屏幕。

下面给出各函数文件

2.mpu_iic.c/mpu_iic.h

mpu_iic.h
主要是宏定义对引脚电平的操作和进行函数声明。

#ifndef __MPU_IIC_H
#define __MPU_IIC_H #include "stm32f10x.h"
#include "delay.h" /* 宏定义引脚电平操作函数 */
#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;} #define MPU_IIC_SDA_1() GPIO_SetBits( GPIOB, GPIO_Pin_11 )
#define MPU_IIC_SDA_0() GPIO_ResetBits( GPIOB, GPIO_Pin_11 ) #define MPU_IIC_SCL_1() GPIO_SetBits( GPIOB, GPIO_Pin_10 )
#define MPU_IIC_SCL_0() GPIO_ResetBits( GPIOB, GPIO_Pin_10 ) #define MPU_IIC_AD0_1() GPIO_SetBits( GPIOA, GPIO_Pin_15 )
#define MPU_IIC_AD0_0() GPIO_ResetBits( GPIOA, GPIO_Pin_15 ) #define MPU_IIC_SDA_READ() GPIO_ReadInputDataBit( GPIOB, GPIO_Pin_11 ) #define MPU_IIC_Delay() delay_us(2) /* 函数声明 */
void MPU_IIC_Init( void );
void MPU_IIC_Start( void );
void MPU_IIC_Stop( void );
uint8_t MPU_IIC_Wait_Ack( void );
void MPU_IIC_Ack( void );
void MPU_IIC_NAck( void );
void MPU_IIC_Send_Byte( uint8_t data );
uint8_t MPU_IIC_Read_Byte( uint8_t ack ); #endif

mpu_iic.c
通过软件模拟IIC的代码,没什么好说的。

#include "mpu_iic.h"
#include "usart.h" /*
IIC接口引脚配置
SDA:PB11
SCL:PB10
AD0:PB2
*/
void MPU_IIC_Init( void )
{
GPIO_InitTypeDef GPIO_InitStruct; RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE ); GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;
GPIO_Init( GPIOB, &GPIO_InitStruct ); MPU_IIC_SDA_1();
MPU_IIC_SCL_1(); } void MPU_IIC_Start( void )
{
MPU_SDA_OUT(); MPU_IIC_SDA_1();
MPU_IIC_SCL_1();
delay_us(2);
MPU_IIC_SDA_0();
delay_us(2);
MPU_IIC_SCL_0();
} void MPU_IIC_Stop( void )
{
MPU_SDA_OUT(); MPU_IIC_SDA_0();
MPU_IIC_SCL_1();
delay_us(2);
MPU_IIC_SDA_1();
MPU_IIC_SCL_1();
delay_us(2); } /* 由从设备在SCL为高电平的时候拉低SDA作为应答
返回值:1:未应答
0:已应答
*/
uint8_t MPU_IIC_Wait_Ack( void )
{
uint8_t count;
MPU_SDA_IN(); MPU_IIC_SCL_1();
delay_us(2);
MPU_IIC_SDA_1();
delay_us(2); while( MPU_IIC_SDA_READ()==1 )
{
count++;
if( count>250 )
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_SCL_0();
return 0;
} void MPU_IIC_Ack( void )
{ MPU_IIC_SCL_0();
MPU_SDA_OUT();
MPU_IIC_SDA_0();
delay_us(2);
MPU_IIC_SCL_1();
delay_us(2);
MPU_IIC_SCL_0(); } void MPU_IIC_NAck( void )
{ MPU_IIC_SCL_0();
MPU_SDA_OUT();
MPU_IIC_SDA_1();
delay_us(2);
MPU_IIC_SCL_1();
delay_us(2);
MPU_IIC_SCL_0();
} /* 发送一个字节数据,高位先行 */
void MPU_IIC_Send_Byte( uint8_t data )
{
uint8_t t;
MPU_SDA_OUT(); MPU_IIC_SCL_0();
for( t=0;t<8;t++ )
{
if( ((data&0x80)>>7)==1 )
MPU_IIC_SDA_1();
else
MPU_IIC_SDA_0();
data<<=1;
MPU_IIC_SCL_1();
delay_us(2);
MPU_IIC_SCL_0();
delay_us(2);
}
} /* 读取一个字节,ack=1时,读取完成后主机发送应答 */
uint8_t MPU_IIC_Read_Byte( uint8_t ack )
{
uint8_t t,data=0;
MPU_SDA_IN();
for( t=0;t<8;t++ )
{
MPU_IIC_SCL_0();
delay_us(2);//等待SDA的变化
MPU_IIC_SCL_1(); data<<=1;//必须在读取前面,因为之后一位读取后就不再移位
if( MPU_IIC_SDA_READ()==1 )
data++; delay_us(2);//等待SDA的变化 } if( !ack )
MPU_IIC_NAck();//发送nACK
else
MPU_IIC_Ack(); //发送ACK
return data;
}

3.mpu6050.c/mpu6050.h

mpu6050.h
主要是定义MPU相关寄存器的地址,和进行函数声明。

#ifndef __MPU6050_H
#define __MPU6050_H #include "stm32f10x.h"
#include "mpu_iic.h" /* AD0接地,MPU6050的IIC地址为0x68 接3.3V就为0x69*/
#define MPU_ADDR 0X68 /************** MPU6050相关寄存器地址 *********************/
#define MPU_ACCEL_OFFS_REG 0X06 //accel_offs寄存器,可读取版本号,寄存器手册未提到
#define MPU_PROD_ID_REG 0X0C //prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG 0X0D //自检寄存器X
#define MPU_SELF_TESTY_REG 0X0E //自检寄存器Y
#define MPU_SELF_TESTZ_REG 0X0F //自检寄存器Z
#define MPU_SELF_TESTA_REG 0X10 //自检寄存器A
#define MPU_SAMPLE_RATE_REG 0X19 //采样频率分频器
#define MPU_CFG_REG 0X1A //配置寄存器
#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG 0X1C //加速度计配置寄存器
#define MPU_MOTION_DET_REG 0X1F //运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG 0X23 //FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG 0X24 //IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG 0X26 //IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG 0X29 //IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG 0X2C //IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG 0X2F //IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG 0X32 //IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG 0X33 //IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG 0X35 //IIC从机4读数据寄存器 #define MPU_I2CMST_STA_REG 0X36 //IIC主机状态寄存器
#define MPU_INTBP_CFG_REG 0X37 //中断/旁路设置寄存器
#define MPU_INT_EN_REG 0X38 //中断使能寄存器
#define MPU_INT_STA_REG 0X3A //中断状态寄存器 #define MPU_ACCEL_XOUTH_REG 0X3B //加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG 0X3C //加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG 0X3D //加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG 0X3E //加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG 0X3F //加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG 0X40 //加速度值,Z轴低8位寄存器 #define MPU_TEMP_OUTH_REG 0X41 //温度值高八位寄存器
#define MPU_TEMP_OUTL_REG 0X42 //温度值低8位寄存器 #define MPU_GYRO_XOUTH_REG 0X43 //陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG 0X44 //陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG 0X45 //陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG 0X46 //陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG 0X47 //陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG 0X48 //陀螺仪值,Z轴低8位寄存器 #define MPU_I2CSLV0_DO_REG 0X63 //IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG 0X64 //IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG 0X65 //IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG 0X66 //IIC从机3数据寄存器 #define MPU_I2CMST_DELAY_REG 0X67 //IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG 0X68 //信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG 0X69 //运动检测控制寄存器
#define MPU_USER_CTRL_REG 0X6A //用户控制寄存器
#define MPU_PWR_MGMT1_REG 0X6B //电源管理寄存器1
#define MPU_PWR_MGMT2_REG 0X6C //电源管理寄存器2
#define MPU_FIFO_CNTH_REG 0X72 //FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG 0X73 //FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG 0X74 //FIFO读写寄存器
#define MPU_DEVICE_ID_REG 0X75 //器件ID寄存器 /* 函数声明 */
uint8_t MPU_Read_Byte( uint8_t reg );
uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data );
uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );
uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf ); uint8_t MPU_Init( void ); uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr );
uint8_t MPU_Set_Accel_Fsr( uint8_t fsr );
uint8_t MPU_Set_LPF( uint16_t lpf );
uint8_t MPU_Set_Rate( uint16_t rate );
short MPU_Get_Temperature( void );
uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz );
uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az ); #endif

mpu6050.c
最为重要的一部分代码,包括了对MPU6050的一系列基本配置和读取原始数据的操作,代码都注解的很详细了。
AD0引脚用了PA15,所以要GPIO_Remap_SWJ_JTAGDisable,换个引脚也可以。

#include "mpu6050.h"
#include "usart.h" /**
* @brief :初始化MPU
* @param :None
* @retval :0:初始化完成
*/
uint8_t MPU_Init( void )
{
uint8_t k=1; uint8_t res; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//先使能外设IO PORTA时钟 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; // 端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
MPU_IIC_AD0_0();
MPU_IIC_Init();
/* 设置MPU6050地址为0X68,并初始化IIC总线 */ /* MPU_PWR_MGMT1_REG:电源管理寄存器 */
MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X80 );//复位MPU6050
// printf("读寄存器值:%02X\n",MPU_Read_Byte( MPU_PWR_MGMT1_REG ));
delay_ms(100);
MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X00 );//唤醒MPU6050 /* 设置陀螺仪量程:±2000dps
设置加速度计量程:±2g
设置采样频率:50Hz(低通滤波器频率100Hz) */
MPU_Set_Gyro_Fsr( 3 );
MPU_Set_Accel_Fsr( 0 );
MPU_Set_Rate( 50 ); MPU_Write_Byte( MPU_INT_EN_REG, 0X00 ); //关闭所有中断 MPU_Write_Byte( MPU_USER_CTRL_REG, 0X00 );//关闭IIC主模式
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 ); //读取MPU6050ID
printf("ID:%X\n",res);
/* 确认ID */
if( res==MPU_ADDR )
{
MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X01 );//以PLL X轴作为时钟参考
MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0X00 );//使能陀螺仪和加速度计
MPU_Set_Rate( 50 );
return 0;
}
else
return 1; } /**
* @brief :读一个寄存器的值(8位)
* @param :reg:寄存器地址
* @retval :读到寄存器的数据
*/
uint8_t MPU_Read_Byte( uint8_t reg )
{
uint8_t data; MPU_IIC_Start();
/* 发送MPU6050器件地址,还有写命令(最低位是0) */
MPU_IIC_Send_Byte( (MPU_ADDR<<1)|0 );
MPU_IIC_Wait_Ack();
/* 写入寄存器地址 */
MPU_IIC_Send_Byte( reg );
MPU_IIC_Wait_Ack(); MPU_IIC_Start();
/* 对MPU6050发送读命令 */
MPU_IIC_Send_Byte( (MPU_ADDR<<1)|1 );
MPU_IIC_Wait_Ack();
/* 读取寄存器的值 */
data = MPU_IIC_Read_Byte( 0 );
MPU_IIC_Stop();
return data;
} /**
* @brief :在指定寄存器中写入数据(8位)
* @param :reg:寄存器地址
data:要写入寄存器的数据
* @retval :正常返回0
*/
uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data )
{
MPU_IIC_Start();
/* 发送MPU6050器件地址,还有写命令(最低位是0) */
MPU_IIC_Send_Byte( (MPU_ADDR<<1)|0 );
if( MPU_IIC_Wait_Ack() )
{
MPU_IIC_Stop();
return 1;
}
/* 写入寄存器地址 */
MPU_IIC_Send_Byte( reg );
MPU_IIC_Wait_Ack(); /* 发送要写入的数据 */
MPU_IIC_Send_Byte( data );
if( MPU_IIC_Wait_Ack() )
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
} /**
* @brief :连续读取寄存器中的数据
* @param :reg:寄存器地址
len:要读取数据的长度(以Byte为单位)
*buf:存储读取到的数据
* @retval :正常返回0
*/
uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf )
{ MPU_IIC_Start();
/* 发送MPU6050器件地址,还有写命令(最低位是0) */
MPU_IIC_Send_Byte( (addr<<1)|0 );
if( MPU_IIC_Wait_Ack() )
{
MPU_IIC_Stop();
return 1;
}
/* 写入寄存器地址 */
MPU_IIC_Send_Byte( reg );
MPU_IIC_Wait_Ack(); MPU_IIC_Start();
/* 对MPU6050发送读命令 */
MPU_IIC_Send_Byte( (MPU_ADDR<<1)|1 );
MPU_IIC_Wait_Ack();
while( len )
{
/* 如果只读一位,不发送应答 */
if( len==1 )
*buf = MPU_IIC_Read_Byte( 0 );
else
*buf = MPU_IIC_Read_Byte( 1 );
len--;
buf++;
}
MPU_IIC_Stop();
return 0;
} /**
* @brief :连续在寄存器中写入数据
* @param :reg:寄存器地址
len:要写入数据的长度(以Byte为单位)
*buf:要写入寄存器的数据
* @retval :正常返回0
*/
uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf )
{
uint8_t i; MPU_IIC_Start();
/* 发送MPU6050器件地址,还有写命令(最低位是0) */
MPU_IIC_Send_Byte( (addr<<1)|0 );
if( MPU_IIC_Wait_Ack() )
{
MPU_IIC_Stop();
return 1;
}
/* 写入寄存器地址 */
MPU_IIC_Send_Byte( reg );
MPU_IIC_Wait_Ack(); i=0;
while( len )
{
MPU_IIC_Send_Byte( buf[i] );
i++;
if( MPU_IIC_Wait_Ack() )
{
MPU_IIC_Stop();
return 1;
}
len--;
} MPU_IIC_Stop();
return 0;
} /**
* @brief :设置陀螺仪量程
* @param :fsr:0:±250dps
1:±500dps
2:±1000dps
3:±2000dps
* @retval :0:设置成功
1:设置失败
*/
uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr )
{
return MPU_Write_Byte( MPU_GYRO_CFG_REG, fsr<<3 );
} /**
* @brief :设置加速度计量程
* @param :fsr:0:±2g
1:±4g
2:±8g
3:±16g
* @retval :0:设置成功
1:设置失败
*/
uint8_t MPU_Set_Accel_Fsr( uint8_t fsr )
{
return MPU_Write_Byte( MPU_ACCEL_CFG_REG, fsr<<3 );
} /**
* @brief :设置低通滤波器频率
* @param :Hz:频率
* @retval :0:设置成功
1:设置失败
*/
uint8_t MPU_Set_LPF( uint16_t lpf )
{
uint8_t data=0; if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);
} /**
* @brief :设置采样频率
* @param :Hz:4~1000Hz
* @retval :0:设置成功
1:设置失败
*/
uint8_t MPU_Set_Rate( uint16_t rate )
{
uint8_t data;
if(rate>1000)
rate=1000;
if(rate<4)
rate=4;
data=1000/rate-1;
/* 采样频率 */
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
} /**
* @brief :获取温度值
* @param :None
* @retval :扩大了100倍的温度值(实际上为了保留俩位小数)
*/
short MPU_Get_Temperature( void )
{
uint8_t buf[2];
uint16_t raw;//存储原始温度值
float temp; /* MPU_TEMP_OUTH_REG:温度值的高八位寄存器0X41
MPU_TEMP_OUTL_REG:温度值的低八位寄存器0X42
连续读出16位温度值*/
MPU_Read_Continue( MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf );
/* 读取原始温度值 */
raw = ( ( uint16_t )buf[0]<<8 )|buf[1];
/* 转换温度值 */
temp = 36.53+( (double)raw )/340; /* 保留俩位小数,小数转换为整数时会舍去小数部分 */
return temp*100;
} /**
* @brief :获取陀螺仪值
* @param :gx、gy、gz:是陀螺仪x、y、z轴的原始读数(16位)
* @retval :0:获取成功
1:获取失败
*/
uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz )
{
uint8_t buf[6],res; /* MPU_GYRO_XOUTH_REG:x轴高八位寄存器
三个轴的寄存器地址是连续的,先高位,后低位 */
if( (res=MPU_Read_Continue( MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf ))==0 )
{
*gx = ((uint16_t)buf[0]<<8)|buf[1];
*gy = ((uint16_t)buf[2]<<8)|buf[3];
*gz = ((uint16_t)buf[4]<<8)|buf[5];
} return res;
} /**
* @brief :获取加速度计值
* @param :ax、ay、az:是加速度计x、y、z轴的原始读数(16位)
* @retval :0:获取成功
1:获取失败
*/
uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az )
{
uint8_t buf[6],res; /* MPU_ACCEL_XOUTH_REG:x轴高八位寄存器
三个轴的寄存器地址是连续的,先高位,后低位 */
if( (res=MPU_Read_Continue( MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf ))==0 )
{
*ax = ((uint16_t)buf[0]<<8)|buf[1];
*ay = ((uint16_t)buf[2]<<8)|buf[3];
*az = ((uint16_t)buf[4]<<8)|buf[5];
} return res;
}

4.DMP相关代码

要想使用DMP求欧拉角的代码,包含下面这几个文件即可,下面列出接口函数,到时候使用时直接使用接口函数即可。

向DMP算法提供的接口宏定义
只需要提供:对MPU6050的读写操作函数和延时函数即可

#define i2c_write   MPU_Write_Continue
#define i2c_read MPU_Read_Continue
#define delay_ms delay_ms

DMP初始化

//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_init(void)
{
uint8_t res=0;
MPU_IIC_Init(); //初始化IIC总线
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
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功能
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(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}else return 10;
return 0;
}

获取欧拉角

//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
return 0;
}

5.mian()函数

主函数中对MPU6050进行初始化后,对DMP也进行初始化,然后就可以直接使用mpu_dmp_get_data()获取欧拉角,还可以获取温度值。

#include "stm32f10x.h"
#include "usart.h"
#include "delay.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" int main(void)
{
uint8_t x=0;
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
short temp; //温度 NVIC_PriorityGroupConfig( 2 );
delay_init();
USART1_Init(115200);
printf("程序开始\n"); if( MPU_Init()!=0 )
{
printf("MPU6050初始化错误!\n");
return 0;
} if( mpu_dmp_init() )
{
printf("DMP初始化错误!\n");
return 0;
}
while(1)
{
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
}
delay_ms(100);
printf("pitch:%02f roll:%02f yaw:%02f\n",pitch,roll,yaw);
} }

STM32—驱动六轴MPU6050输出欧拉角的更多相关文章

  1. 如何利用小熊派获取MPU6050六轴原始数据

    摘要:使用小熊派开发板,通过硬件IIC与MPU6050六轴传感器模块通信,完成相应寄存器配置,成功获取陀螺仪.加速度计数据. 本问主要讲述使用小熊派开发板+MPU6050六轴传感器,获取加速度计以及陀 ...

  2. 【.NET 与树莓派】六轴飞控传感器(MPU 6050)

    所谓"飞控",其实是重力加速度计和陀螺仪的组合,因为多用于控制飞行器的平衡(无人机.遥控飞机).有同学会问,这货为什么会有六轴呢?咱们常见的不是X.Y.Z三轴吗?重力加速度有三轴, ...

  3. STM32—驱动GY85-IMU模块

    GY85是一个惯性测量模块,内部集成了三轴加速度计.三轴陀螺仪.电子罗盘.气压传感器等芯片,用于测量和报告设备速度.方向.重力,模块可以将加速度计.陀螺仪.电子罗盘等传感器的数据进行综合,在上位机可以 ...

  4. STM32的GPIO口的输出开漏输出和推挽输出

    本文来自cairang45的博客,讲述了STM32的GPIO口的输出开漏输出和推挽输出, 作者博客:http://blog.ednchina.com/cairang45 本文来自: 高校自动化网(Ww ...

  5. STM32驱动DS18B20

    DS18B20 是由 DALLAS 半导体公司推出的一种的“一线总线”接口的温度传感器.与传 统的热敏电阻等测温元件相比,它是一种新型的体积小.适用电压宽.与微处理器接口简单的 数字化温度传感器.一线 ...

  6. STM32驱动OV7725摄像头颜色识别

    实验目的: 使用stm32驱动OV7725摄像头进行图像实时采集,在tft屏幕上实时显示并识别图像中的特定颜色,在颜色的周围画上框. 实验现象: 我的工程代码链接: http://download.c ...

  7. 字符设备驱动(六)按键poll机制

    title: 字符设备驱动(六)按键poll机制 tags: linux date: 2018-11-23 18:57:40 toc: true --- 字符设备驱动(六)按键poll机制 引入 在字 ...

  8. STM32驱动ILI9341控制器控制TFTLCD显示

    STM32驱动ILI9341控制器控制TFTLCD显示 一.用STM32控制TFTLCD显示的编程方法,在编程驱动TFTLCD液晶显示器之前,我们先熟悉以下概念: 1.色彩深度,这是一个与TFTLCD ...

  9. AMS5601的ardunio和STM32驱动开发

    AMS5601的ardunio和STM32驱动开发 本文有麦粒电子撰写,并提供相应产品服务. 前言 目前ams关于磁编码芯片用的比较多的可能是ams5600,能够输出pwm信号,电压信号以及I2C通信 ...

随机推荐

  1. 12 shell case in语句

    Shell也支持两种分支结构(选择结构),分别是 if else 语句和 case in 语句.当分支较多,并且判断条件比较简单时,使用 case in 语句就比较方便了. if else 语句与ca ...

  2. 『无为则无心』Python函数 — 28、Python函数的简单应用

    目录 1.函数嵌套调用 2.Python函数的简单应用 (1)打印线条 (2)函数计算 (3)打印图形 3.函数的说明文档 (1)函数的说明文档的作用 (2)函数说明文档的语法 (3)查看函数的说明文 ...

  3. 深入理解C++11 阅读笔记

    二 保证稳定性和兼容性保持与C99兼容 预定义宏 C99语言标准增加的一些预定义宏,C++11同样增加了对这些宏的支持 __func__预定义标识符 功能是返回所在函数的名字,在C++11中,标准甚至 ...

  4. libzip开发笔记(二):libzip库介绍、ubuntu平台编译和工程模板

    前言   Qt使用一些压缩解压功能,选择libzip库,libzip库比较原始,也是很多其他库的基础支撑库,编译过了windows版本,有需求编译一个ubuntu版本的,交叉编译需求的同样可参照本文章 ...

  5. 详解Lombok中的@Builder用法

    Builder 使用创建者模式又叫建造者模式.简单来说,就是一步步创建一个对象,它对用户屏蔽了里面构建的细节,但却可以精细地控制对象的构造过程. 基础使用 @Builder注释为你的类生成相对略微复杂 ...

  6. Java8 DateTime API

    Java 8的日期/时间API,有篇不错的文章,直接转载 原文链接: journaldev 翻译: ImportNew.com- Justin Wu译文链接: http://www.importnew ...

  7. 【LeetCode】404. 左叶子之和

    404. 左叶子之和 知识点:二叉树 题目描述 计算给定二叉树的所有左叶子之和.. 示例 3 / \ 9 20 / \ 15 7 在这个二叉树中,有两个左叶子,分别是 9 和 15,所以返回 24 解 ...

  8. LC-322. 零钱兑换

    322. 零钱兑换 给你一个整数数组 coins ,表示不同面额的硬币:以及一个整数 amount ,表示总金额. 计算并返回可以凑成总金额所需的 最少的硬币个数 .如果没有任何一种硬币组合能组成总金 ...

  9. 【codeforces1058】Vasya and Golden Ticket 枚举+暴力+模拟

    #点击传送 题目描述 Recently Vasya found a golden ticket - a sequence which consists of nn digits a1a2-ana1a2 ...

  10. odoo里面条件写法

    attrs="{'invisible': ['|', ('probability', '>', 0), ('active', '=', True)]}"/>. 写法gt ...