07-惯性测量单元（IMU）

概述

惯性测量单元（Inertial Measurement Unit, IMU）是一种集成了多个传感器的设备，用于测量和报告物体的运动状态，包括角速度、线性加速度和磁场信息。IMU通常包含以下三种主要传感器：

1. 陀螺仪（Gyroscope）：陀螺仪主要用于测量物体的角速度，即物体绕某个轴的旋转速度。陀螺仪的工作原理基于角动量守恒定律，可以用于测量或维持物体的旋转状态。
2. 加速度计（Accelerometer）：加速度计用于测量物体的线性加速度，即物体在直线路径上的加速度。加速度计可以检测到由于重力、运动或震动引起的加速度变化。这些信息有助于确定物体的运动状态和姿态。
3. 磁力计（Magnetometer）：磁力计用于测量地磁场的强度和方向。通过与加速度计和陀螺仪的数据相结合，可以进一步提高姿态估计的准确性，特别是在长时间运动过程中。

IMU通过将这些传感器的数据进行处理和融合，可以提供物体的运动状态信息，如姿态（物体在空间中的方向）、位置和速度。在实际应用中，IMU常用于无人机、机器人、航天器、汽车等领域，用于实现导航、定位、稳定控制等功能。为了获得更准确的运动状态信息，通常会使用传感器融合算法（如卡尔曼滤波器）对各个传感器的数据进行处理和融合。

下面的两张图,简单演示了加速度计与陀螺仪的工作原理

加速度计

陀螺仪

mpu6050

MPU-6050是一款由InvenSense公司生产的六轴运动处理单元（Motion Processing Unit, MPU），集成了一个三轴陀螺仪和一个三轴加速度计。MPU-6050可以用于测量物体的角速度和线性加速度，从而获取物体的运动状态信息。
MPU-6050的主要特点如下：

1. 集成传感器：MPU-6050将三轴陀螺仪和三轴加速度计集成在一个芯片上，节省了空间和成本。这使得MPU-6050成为一款非常适合用于微型和低功耗设备的运动传感器。
2. 数字输出：MPU-6050具有I2C接口，可以直接与微控制器（如Arduino）通信。这使得数据读取和处理变得相对简单。
3. 内置数字运动处理器（Digital Motion Processor, DMP）：MPU-6050内置了一个数字运动处理器，可以实现一些基本的传感器数据处理和融合功能。使用DMP可以减轻主控制器的计算负担，提高运动状态估计的准确性。
4. 可配置：MPU-6050允许用户配置陀螺仪和加速度计的量程和灵敏度，以适应不同的应用场景和需求。

MPU-6050广泛应用于无人机、机器人、运动控制、虚拟现实、游戏控制器等领域。使用MPU-6050结合适当的数据处理和传感器融合算法，可以实现对物体的姿态、位置和速度的实时监测和控制。

mpu6050驱动代码

mpu6050.h代码

#ifndef __MPU6050_H
#define __MPU6050_H
#include <stdint.h>
//#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寄存器
//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
//如果接V3.3,则IIC地址为0X69(不包含最低位).
#define MPU_ADDR                0X68
////因为模块AD0默认接GND,所以转为读写地址后,为0XD1和0XD0(如果接VCC,则为0XD3和0XD2)  
//#define MPU_READ    0XD1
//#define MPU_WRITE   0XD0
uint8_t IIC_Read_Byte(uint8_t dev_addr, uint8_t reg);
uint8_t MPU_Init(void);                                 //初始化MPU6050
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf);//IIC连续写
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf); //IIC连续读 
//uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data);                //IIC写一个字节
//uint8_t MPU_Read_Byte(uint8_t reg);                        //IIC读一个字节
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);
uint8_t MPU_Set_Fifo(uint8_t sens);
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代码

#include "mpu6050.h"
#include "i2c.h"
#include "systick.h"
#include <stdio.h>
//初始化MPU6050
//返回值:0,成功
//其他,错误代码
uint8_t MPU_Init(void)
{ 
    uint8_t res; 
    //MPU_IIC_Init();//初始化IIC总线
    //MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);    //复位MPU6050
    IIC_Init(); 
     delay_1ms(100);
    //IIC_SearchAddr();
    printf("MPU_Init start...\r\n");
    uint8_t ret = IIC_WhoAmI(MPU_ADDR);
    //printf("who am i %d \r\n",ret);
        //复位MPU6050
    IIC_Send_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG,0x80);    
  delay_1ms(50);
    //MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);    //唤醒MPU6050
    IIC_Send_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG,0x00);    
    MPU_Set_Gyro_Fsr(3);                    //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);                    //加速度传感器,±2g
    MPU_Set_Rate(200);                        //设置采样率50Hz
    IIC_Send_Byte(MPU_ADDR,MPU_INT_EN_REG,0X00);    //关闭所有中断
    IIC_Send_Byte(MPU_ADDR,MPU_USER_CTRL_REG,0X00);    //I2C主模式关闭
    IIC_Send_Byte(MPU_ADDR,MPU_FIFO_EN_REG,0X00);    //开启FIFO
    IIC_Send_Byte(MPU_ADDR,MPU_INTBP_CFG_REG,0X80);    //INT引脚低电平有效
    res=IIC_Read_Byte(MPU_ADDR,MPU_DEVICE_ID_REG);
    if(res==MPU_ADDR)//器件ID正确
    {
        IIC_Send_Byte(MPU_ADDR,MPU_PWR_MGMT1_REG,0X01);    //设置CLKSEL,PLL X轴为参考
        IIC_Send_Byte(MPU_ADDR,MPU_PWR_MGMT2_REG,0X00);    //加速度与陀螺仪都工作
        MPU_Set_Rate(200);                        //设置采样率为50Hz
     }else return 1;
    printf("mpu6050 init complete\r\n");
    return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    //return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围 
    return IIC_Send_Byte(MPU_ADDR, MPU_GYRO_CFG_REG,fsr<<3);    
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    //return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
    return IIC_Send_Byte(MPU_ADDR, MPU_ACCEL_CFG_REG,fsr<<3);    
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
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);//设置数字低通滤波器  
    return IIC_Send_Byte(MPU_ADDR, MPU_CFG_REG,data);    
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
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);    //设置数字低通滤波器
    data = IIC_Send_Byte(MPU_ADDR, MPU_SAMPLE_RATE_REG,data);    
     return MPU_Set_LPF(rate/2);    //自动设置LPF为采样率的一半
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    uint8_t buf[2]; 
    short raw;
    float temp;
    //MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    IIC_ReadData(MPU_ADDR, MPU_TEMP_OUTH_REG,buf, 2);
    raw=((uint16_t)buf[0]<<8)|buf[1];  
    temp=36.53+((double)raw)/340;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;  
    //res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    res = IIC_ReadData(MPU_ADDR, MPU_GYRO_XOUTH_REG,buf, 6);
    if(res==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;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;  
    //res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
    res = IIC_ReadData(MPU_ADDR, MPU_ACCEL_XOUTH_REG,buf, 6);
    if(res==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;;
}
//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
        return IIC_WriteData( addr,  reg, buf, len);
} 
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{ 
        return IIC_ReadData( addr,  reg, buf, len);
}

mpu6050中的DMP库

为了简化开发者的工作，MPU6050提供了一个称为DMP（Digital Motion Processor）的库，用于处理和融合陀螺仪和加速度计的原始数据，从而提供更高级别的运动追踪和姿态估计功能。
DMP库通过执行内部的传感器融合算法，可以实现一些高级功能，比如自动姿态追踪。开发者可以通过配置DMP库的参数和使用提供的API接口，来使用这些功能而不必编写复杂的传感器融合算法。DMP库还提供了一些附加的功能，如获取方向角、线性加速度和重力加速度等。

以下是使用dmp库的一般步骤:

1. 初始化I2C接口
2. 引入inv_mpu.h头文件
3. 调用mpu_dmp_init()函数,完成dmp库的初始化
4. 通过mpu_dmp_get_data(&pitch,&roll,&yaw)去获取IMU当前的方向角
5. 通过mpu_get_temperature(&temp,NULL);可以获取到当前环境温度

注意在这里使用的时候,需要注意这些函数返回的数据类型, 尤其是数据格式.
例如它的温度数据是采用Q16格式进行返回的. 那么我们就涉及到Q16类型的数据还原成原来的数据

Q16格式是一种固定点数表示法，常用于将浮点数表示为固定位数的整数。在Q16格式中，数值被表示为一个16位的整数，其中14位用于表示整数部分，2位用于表示小数部分。
Q16格式的数值范围取决于整数部分的位数，而小数部分的位数决定了精度。在Q16格式中，整数部分的范围为[-32768, 32767]，小数部分的精度为2^-16。
Q16格式通过将浮点数乘以2^16，将其转换为整数表示。例如，浮点数1.0可以表示为Q16格式的整数值65536（即1.0 2^16），浮点数0.5可以表示为Q16格式的整数值32768（即0.5 2^16）。
Q16格式在嵌入式系统和数字信号处理等领域常被使用，因为它可以在固定的位数上提供一定的精度，并且可以使用整数运算来进行计算，避免了浮点数运算的复杂性和开销。

用Q16格式的数据,除以2^16即可得到原始的浮点数据

float q16ToFloat(int q16Value) {
    float floatValue = (float)q16Value / (float)(1 << 16);
    return floatValue;
}