STM32入门笔记(02)：MPU6050、MPU9250、ICM20948及姿态解算（SPL库函数版)

---

MPU6050

【正点原子】 手把手教你学STM32 系列视频之 STM32F4-基于探索者F407 DMP 读取MPU6050角度数据以及匿名四轴飞控上位机介绍

STM32F103 DMP读取MPU6050角度数据

什么是MPU6050?

MPU6050是InvenSense公司( Invensense公司成立于2003年6月，总部位于美国Sunnyvale，主要生产的产品为运动感测追踪组件。) 推出的全球首款整合性6轴运动处理组件，内带3轴陀螺仪和3轴加速度传感器，并且含有一个第二IIC接口，可用于连接外部磁力传感器，利用自带数字运动处理器DMP: Digital Motion Processor）硬件加速引擎，通过主IIC接口，可以向应用端输出完整的9轴姿态融合演算数据。

有了DMP，我们可以使用InvenSense公司提供的运动处理资料库，非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度 。

MPU6050的特点

• ①自带数字运动处理（ DMP: Digital Motion Processing ），可以输出6轴或9轴（需外接磁传感器）姿态解算数据。
• ②集成可程序控制，测量范围为±250、±500、±1000与±2000°/sec 的3轴角速度感测器(陀螺仪)
• ③集成可程序控制，范围为±2g、±4g、±8g和±16g的3轴加速度传感器
• ④自带数字温度传感器
• ⑤可输出中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G中断、零动作感应、触击感应、摇动感应功能
• ⑥自带1024字节FIFO，有助于降低系统功耗
• ⑦高达400Khz的IIC通信接口
• ⑧超小封装尺寸：4x4x0.9mm（QFN）

MPU6050框图

AD0接GND 地址为 0x68
AD0接VCC 地址为 0x69

MPU6050初始化

mpu6050.c

u8 MPU_Init(void)
{ 
	u8 res;
	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;
}

MPU6050寄存器

电源管理寄存器1（0X6B）

陀螺仪配置寄存器（0X1B）

加速度传感器配置寄存器（0X1C）

FIFO使能寄存器（0X23）

陀螺仪采样率分频寄存器（0X19）

配置寄存器（0X1A）

设置：带宽=1/2采样率

电源管理寄存器2（0X6C）

加速度传感器数据输出寄存器（0X3B~0X40）

陀螺仪数据输出寄存器（0X43~0X48）

温度传感器数据输出寄存器（0X41~0X42）

通过前面的学习，我们可以正常读取MPU6050的加速度传感器、陀螺仪和温度传感器的数据，但是实际使用的时候（比如做四轴），我们更希望得到姿态数据，即欧拉角：

• 俯仰角（pitch）
• 横滚角（roll）
• 航向角（yaw）

要得到欧拉角数据，就得利用传感器获取到的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，初学者不易掌握。

【B站@沁恒微电子】赤菟V307应用方案分享之IMU姿态解析

通过上面两个坐标系，可以描述物体的姿态信息，通常描述物体的姿态方式如下：

• 1.欧拉角
• 2.旋转矩阵
• 3.四元数

而MPU6050自带了数字运动处理器，即DMP，并且，InvenSense提供了一个MPU6050的嵌入式运动驱动库，结合MPU6050的DMP，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到yaw、roll和pitch。

使用内置的DMP，可以大大简化代码设计，MCU不用进行姿态解算过程，大大降低了MCU的负担，从而有更多的时间去处理其他事件，提高系统实时性。

DMP使用介绍

MPU6050 DMP输出的是姿态解算后的四元数，采用q30格式，也就是放大了2的30次方，我们要得到欧拉角，就得做一个转换，代码如下：

参考：【CSDN@xiaoma_bk】四元数与欧拉角（Yaw、Pitch、Roll）的转换

---

正弦曲线

正弦函数

C语言实现正弦函数

/* sin example */
#include <stdio.h>      /* printf */
#include <math.h>       /* sin */
#define PI 3.14159265
int main ()
{
	int i;
    float  param, result;
    param = 30.0;
    
	for(i = 0; i<=90;i++)
	{
		    result = sin (i*PI/180);//sin0°~sin90° 
            printf ("The sine of %d degrees is %f.\n", i, result );
	 } 
	 
	result = sin (param*PI/180);//sin30° 
    printf ("The sine of %f degrees is %f.\n", param, result );
    return 0;
}

代码说明 ：

π = 3.1415926

1° = 3.1415926 / 180 = 0.01745 弧度

30° = 30*0.01745 =0.5 弧度 = 1/2

sin (i*PI/180);

i 取值0°~90° ，换算成弧度的sin结果范围为 0~1。

sin (param*PI/180);

param 取值0°、30°、90°，换算成弧度的sin结果为：
sin(0°) = 0
sin(30°)=1/2
sin(90°)=1

弧度与角度换算公式:

---

//q30格式,long转float时的除数.
#define q30  1073741824.0f

//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};
//MPU6050自测试
//返回值:0,正常
//    其他,失败
u8 run_self_test(void)
{
	int result;
	//char test_packet[4] = {0};
	long gyro[3], accel[3]; 
	result = mpu_run_self_test(gyro, accel);
	if (result == 0x3) 
	{
		/* Test passed. We can trust the gyro data here, so let's push it down
		* to the DMP.
		*/
		float sens;
		unsigned short accel_sens;
		mpu_get_gyro_sens(&sens);
		gyro[0] = (long)(gyro[0] * sens);
		gyro[1] = (long)(gyro[1] * sens);
		gyro[2] = (long)(gyro[2] * sens);
		dmp_set_gyro_bias(gyro);
		mpu_get_accel_sens(&accel_sens);
		accel[0] *= accel_sens;
		accel[1] *= accel_sens;
		accel[2] *= accel_sens;
		dmp_set_accel_bias(accel);
		return 0;
	}else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar; 
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{

}
//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
u8 mpu_dmp_init(void)
{
	u8 res=0;
	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;     
	}
	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,正常
//    其他,失败
u8 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;
}

硬件连接

MPU6050 与单片机 IIC 通信

SCL -----()
SDA -----()

略

软件设计

MPU6050驱动代码

1.MPU6050 IIC接口驱动代码
2.MPU_Init函数
3.MPU_Get_Gyroscope函数
4.MPU_Get_Accelerometer函数
5.MPU_Get_Temperature函数

#include "mpu6050.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"   
//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
u8 MPU_Init(void)
{ 
	u8 res;
	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;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_LPF(u16 lpf)
{
	u8 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);//设置数字低通滤波器  
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Rate(u16 rate)
{
	u8 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为采样率的一半
}




//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码

u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz) 
{
    u8 buf[6],res;  
	res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(res==0)
	{
		*gx=((u16)buf[0]<<8)|buf[1];  
		*gy=((u16)buf[2]<<8)|buf[3];  
		*gz=((u16)buf[4]<<8)|buf[5];
	} 	
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    u8 buf[6],res;  
	res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(res==0)
	{
		*ax=((u16)buf[0]<<8)|buf[1];  
		*ay=((u16)buf[2]<<8)|buf[3];  
		*az=((u16)buf[4]<<8)|buf[5];
	} 	
    return res;;
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2]; 
    short raw;
	float temp;
	MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((u16)buf[0]<<8)|buf[1];  
    temp=36.53+((double)raw)/340;  
    return temp*100;;
}

//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
	u8 i; 
    IIC_Start(); 
	IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令	
	if(IIC_Wait_Ack())	//等待应答
	{
		IIC_Stop();		 
		return 1;		
	}
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答
	for(i=0;i<len;i++)
	{
		IIC_Send_Byte(buf[i]);	//发送数据
		if(IIC_Wait_Ack())		//等待ACK
		{
			IIC_Stop();	 
			return 1;		 
		}		
	}    
    IIC_Stop();	 
	return 0;	
} 
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{ 
 	IIC_Start(); 
	IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令	
	if(IIC_Wait_Ack())	//等待应答
	{
		IIC_Stop();		 
		return 1;		
	}
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答
    IIC_Start();
	IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令	
    IIC_Wait_Ack();		//等待应答 
	while(len)
	{
		if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK 
		else *buf=IIC_Read_Byte(1);		//读数据,发送ACK  
		len--;
		buf++; 
	}    
    IIC_Stop();	//产生一个停止条件 
	return 0;	
}
//IIC写一个字节 
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data) 				 
{ 
    IIC_Start(); 
	IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令	
	if(IIC_Wait_Ack())	//等待应答
	{
		IIC_Stop();		 
		return 1;		
	}
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答 
	IIC_Send_Byte(data);//发送数据
	if(IIC_Wait_Ack())	//等待ACK
	{
		IIC_Stop();	 
		return 1;		 
	}		 
    IIC_Stop();	 
	return 0;
}
//IIC读一个字节 
//reg:寄存器地址 
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{
	u8 res;
    IIC_Start(); 
	IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令	
	IIC_Wait_Ack();		//等待应答 
    IIC_Send_Byte(reg);	//写寄存器地址
    IIC_Wait_Ack();		//等待应答
    IIC_Start();
	IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令	
    IIC_Wait_Ack();		//等待应答 
	res=IIC_Read_Byte(0);//读取数据,发送nACK 
    IIC_Stop();			//产生一个停止条件 
	return res;		
}

DMP驱动代码

1.DMP移植相关代码
i2c_write、i2c_read、delay_ms和get_ms
2. mpu_dmp_init函数
3.mpu_dmp_get_data函数

inv_mpu_dmp.c

#define i2c_write   MPU_Write_Len
#define i2c_read    MPU_Read_Len
#define delay_ms    delay_ms
#define get_ms      mget_ms

//q30格式,long转float时的除数.
#define q30  1073741824.0f

//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};
//MPU6050自测试
//返回值:0,正常
//    其他,失败
u8 run_self_test(void)
{
	int result;
	//char test_packet[4] = {0};
	long gyro[3], accel[3]; 
	result = mpu_run_self_test(gyro, accel);
	if (result == 0x3) 
	{
		/* Test passed. We can trust the gyro data here, so let's push it down
		* to the DMP.
		*/
		float sens;
		unsigned short accel_sens;
		mpu_get_gyro_sens(&sens);
		gyro[0] = (long)(gyro[0] * sens);
		gyro[1] = (long)(gyro[1] * sens);
		gyro[2] = (long)(gyro[2] * sens);
		dmp_set_gyro_bias(gyro);
		mpu_get_accel_sens(&accel_sens);
		accel[0] *= accel_sens;
		accel[1] *= accel_sens;
		accel[2] *= accel_sens;
		dmp_set_accel_bias(accel);
		return 0;
	}else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar; 
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{

}
//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
u8 mpu_dmp_init(void)
{
	u8 res=0;
	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;     
	}
	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,正常
//    其他,失败
u8 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;
}

四轴上位机

匿名上位机下载地址

匿名上位机V7版–0基础教程1–总体介绍

匿名上位机V7版--0基础教程1--总体介绍

mai.c文件

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "key.h"
#include "lcd.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 


//串口1发送1个字符 
//c:要发送的字符
void usart1_send_char(u8 c)
{

	while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); 
    USART_SendData(USART1,c);   

} 
//传送数据给匿名四轴上位机软件(V2.6版本)
//fun:功能字. 0XA0~0XAF
//data:数据缓存区,最多28字节!!
//len:data区有效数据个数
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
	u8 send_buf[32];
	u8 i;
	if(len>28)return;	//最多28字节数据 
	send_buf[len+3]=0;	//校验数置零
	send_buf[0]=0X88;	//帧头
	send_buf[1]=fun;	//功能字
	send_buf[2]=len;	//数据长度
	for(i=0;i<len;i++)send_buf[3+i]=data[i];			//复制数据
	for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];	//计算校验和	
	for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);	//发送数据到串口1 
}
//发送加速度传感器数据和陀螺仪数据
//aacx,aacy,aacz:x,y,z三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
{
	u8 tbuf[12]; 
	tbuf[0]=(aacx>>8)&0XFF;
	tbuf[1]=aacx&0XFF;
	tbuf[2]=(aacy>>8)&0XFF;
	tbuf[3]=aacy&0XFF;
	tbuf[4]=(aacz>>8)&0XFF;
	tbuf[5]=aacz&0XFF; 
	tbuf[6]=(gyrox>>8)&0XFF;
	tbuf[7]=gyrox&0XFF;
	tbuf[8]=(gyroy>>8)&0XFF;
	tbuf[9]=gyroy&0XFF;
	tbuf[10]=(gyroz>>8)&0XFF;
	tbuf[11]=gyroz&0XFF;
	usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1
}	
//通过串口1上报结算后的姿态数据给电脑
//aacx,aacy,aacz:x,y,z三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
//roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00  ->  180.00度
//pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
//yaw:航向角.单位为0.1度 0 -> 3600  对应 0 -> 360.0度
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
{
	u8 tbuf[28]; 
	u8 i;
	for(i=0;i<28;i++)tbuf[i]=0;//清0
	tbuf[0]=(aacx>>8)&0XFF;
	tbuf[1]=aacx&0XFF;
	tbuf[2]=(aacy>>8)&0XFF;
	tbuf[3]=aacy&0XFF;
	tbuf[4]=(aacz>>8)&0XFF;
	tbuf[5]=aacz&0XFF; 
	tbuf[6]=(gyrox>>8)&0XFF;
	tbuf[7]=gyrox&0XFF;
	tbuf[8]=(gyroy>>8)&0XFF;
	tbuf[9]=gyroy&0XFF;
	tbuf[10]=(gyroz>>8)&0XFF;
	tbuf[11]=gyroz&0XFF;	
	tbuf[18]=(roll>>8)&0XFF;
	tbuf[19]=roll&0XFF;
	tbuf[20]=(pitch>>8)&0XFF;
	tbuf[21]=pitch&0XFF;
	tbuf[22]=(yaw>>8)&0XFF;
	tbuf[23]=yaw&0XFF;
	usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
} 
  
int main(void)
{ 
	u8 t=0,report=1;			//默认开启上报
	u8 key;
	float pitch,roll,yaw; 		//欧拉角
	short aacx,aacy,aacz;		//加速度传感器原始数据
	short gyrox,gyroy,gyroz;	//陀螺仪原始数据
	short temp;					//温度
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
	delay_init(168);  //初始化延时函数
	uart_init(500000);		//初始化串口波特率为500000
	LED_Init();					//初始化LED 
	KEY_Init();					//初始化按键
 	LCD_Init();					//LCD初始化
	MPU_Init();					//初始化MPU6050
 	POINT_COLOR=RED;//设置字体为红色 
	LCD_ShowString(30,50,200,16,16,"Explorer STM32F4");	
	LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");	
	LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
	LCD_ShowString(30,110,200,16,16,"2014/5/9");
	while(mpu_dmp_init())
	{
		LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
		delay_ms(200);
		LCD_Fill(30,130,239,130+16,WHITE);
 		delay_ms(200);
	}
	LCD_ShowString(30,130,200,16,16,"MPU6050 OK");
	LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");
	POINT_COLOR=BLUE;//设置字体为蓝色 
 	LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");	 
 	LCD_ShowString(30,200,200,16,16," Temp:    . C");	
 	LCD_ShowString(30,220,200,16,16,"Pitch:    . C");	
 	LCD_ShowString(30,240,200,16,16," Roll:    . C");	 
 	LCD_ShowString(30,260,200,16,16," Yaw :    . C");	 
 	while(1)
	{
		key=KEY_Scan(0);
		if(key==KEY0_PRES)
		{
			report=!report;
			if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
			else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
		}
		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);	//得到陀螺仪数据
			if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
			if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
			if((t%10)==0)
			{ 
				if(temp<0)
				{
					LCD_ShowChar(30+48,200,'-',16,0);		//显示负号
					temp=-temp;		//转为正数
				}else LCD_ShowChar(30+48,200,' ',16,0);		//去掉负号 
				LCD_ShowNum(30+48+8,200,temp/100,3,16);		//显示整数部分	    
				LCD_ShowNum(30+48+40,200,temp%10,1,16);		//显示小数部分 
				temp=pitch*10;
				if(temp<0)
				{
					LCD_ShowChar(30+48,220,'-',16,0);		//显示负号
					temp=-temp;		//转为正数
				}else LCD_ShowChar(30+48,220,' ',16,0);		//去掉负号 
				LCD_ShowNum(30+48+8,220,temp/10,3,16);		//显示整数部分	    
				LCD_ShowNum(30+48+40,220,temp%10,1,16);		//显示小数部分 
				temp=roll*10;
				if(temp<0)
				{
					LCD_ShowChar(30+48,240,'-',16,0);		//显示负号
					temp=-temp;		//转为正数
				}else LCD_ShowChar(30+48,240,' ',16,0);		//去掉负号 
				LCD_ShowNum(30+48+8,240,temp/10,3,16);		//显示整数部分	    
				LCD_ShowNum(30+48+40,240,temp%10,1,16);		//显示小数部分 
				temp=yaw*10;
				if(temp<0)
				{
					LCD_ShowChar(30+48,260,'-',16,0);		//显示负号
					temp=-temp;		//转为正数
				}else LCD_ShowChar(30+48,260,' ',16,0);		//去掉负号 
				LCD_ShowNum(30+48+8,260,temp/10,3,16);		//显示整数部分	    
				LCD_ShowNum(30+48+40,260,temp%10,1,16);		//显示小数部分  
				t=0;
				LED0=!LED0;//LED闪烁
			}
		}
		t++; 
	} 	
}

MPU6050实验(库函数版) ：标准例程下载

---

MPU9250

参考资料：

• 【华强商城】MPU9250 9轴数字运动处理器_引脚说明_功能规格_应用领域

• 【CSDN@青年云耕】MPU9250介绍 | CSDN@苏导：MPU9250的详细功能

• 【Openeda论坛】【探索者F407MPU9250MPL读取】消除yaw角漂移

【Openeda论坛@Nemesis】基于STM32F4平台的MPU9250MPL方式读取

TDK InvenSense ICM-20948 9轴MEMS MotionTracking™器件是一款多芯片模块 (MCM)，在小型3mm x 3mm x 1mm QFN封装中集成了两个裸片。其中一个裸片设有3轴陀螺仪、3轴加速度计以及Digital Motion Processor™ (DMP)。另一个裸片中含有Asahi Kasei Microdevices Corporation的AK09916 3轴磁力计。

• 陀螺仪的_角速度测量范围最高达±2000（° /s）
• 加速度计的测量范围最大为±16g( g 为_{重力加速度})
• 磁力计的_{磁感应强度}测量范围为±4800μT
  
  磁力计AK8963仅仅是与MPU6050封装在一起构成MPU9250。
  

陀螺仪的可编程满量程范围为±250dps、±500dps、±1000dps和±2000dps。加速度计的用户可编程加速度计满量程范围为±2g、±4g、±8g和±16g。两款传感器的初始灵敏度均经过工厂校准，因此降低了生产线校准要求。

ICM-20948器件设有9轴集成、片上DMP (数字运动处理器: Digital Motion Processor) 和运行时间校准固件。其他特性包括片上16位ADC、可编程数字滤波器、嵌入式温度传感器和可编程中断。该器件设有I2C和SPI串行接口以及独立的数字IO电源，VDD工作范围为1.71V至3.6V，VDDIO为1.71V至1.95V。借助高达100kHz（标准模式）或400kHz（快速模式）的I2C，或者高达7MHz的SPI，可与器件的所有寄存器进行通信。该器件具有20,000g的冲击可靠性，因此十分稳健。

MPU9250中文说明书中提到：当设备处于睡眠状态下，检测到运动时通过9250INT引脚输出电平唤醒主控芯片。

中断的功能可以通过配置中断寄存器来配置。可配置的有： INT 中断引脚配置，中断锁和清
除以及中断触发。产生中断的情况有：

• (1) 时钟振荡改变的时候（通常发生在切换时钟源时 4 发生）
• (2) 有新数据可读的时候（FIFO 寄存器内的数据）
• (3) 加速度的中断功能（运动唤醒功能）
• (4) 没有接收到辅助传感器数据的时候
• 中断的状态可以从中断寄存器中读取。

INT 引脚必须和主控芯片的相连以便唤醒休眠中的主机。

---

ICM20948

MPU9250与ICM20948的区别

MPU-9250/MPU-9255，9轴惯性运动传感器的使用寿命结束；TDK-INVN已于2018年12月31日宣布MPU-9250/MPU-9255下线。

参考文档：https://invensense.tdk.com/wp-content/uploads/2018/10/AN-000146-v2.0-TDK_Migration_MPU_9250toICM-20948.pdf

功能比较 Functional comparison:

硬件比较 Hardware comparison：

硬件注意事项 Hardware Considerations：

1、VDDIO注意事项：

▪ 与MPU-9250相比，ICM-20948的VDDIO范围更低。
▪ 它们在1.71V的低端都兼容。
▪ 然而，在高端，MPU-9250 VDDIO可以高达3.6V，而ICM-20948 VDDIO限制在1.95V。今年五月需要重新设计电源，使VDDIO电压在ICM-20948范围内。
▪ ICM-20948的最大I/O电压为VDDIO+0.5V。高于此值的电压可能会导致芯片行为不当或损坏。为了使I/O电压水平在ICM-20948的范围内，可能需要两种可能的解决方案之一：
（a） 重新设计电源或电源
（b） 增加电平转换器，使连接AP I/O的电压在ICM-20948范围内。

功耗考虑：
▪ 作为一种较新的设计，ICM-20948在所有等效工作模式下的功耗都低于MPU-9250。这个
更低的VDDIO也有助于降低功耗。
▪ 因此，除了上面“VDDIO注意事项”一节中所述的VDDIO不兼容，迁移到ICM-20948
不需要对电源设计进行任何更改。

硬件插入兼容性：

▪ 典型的系统设计可能会使用1.8V的VDDIO，其范围在MPU-9250、ICM-20948以及
I2C/SPI主AP/MCU
▪ 在这种情况下，无需重新设计电源。
▪ 对于这种已经将VDDIO限制在1.71V到1.95V范围内的设计，从硬件角度来看，ICM-20948可以替代MPU9250，而无需任何板/设计更改。
注：不支持在SCL/SCLK和nCS引脚保持低位的情况下通电。万一这通电
如果使用这种方法，则需要在初始化之前使用PWR_管理_1寄存器进行软件重置。

芯片引脚比较 ：

软件注意事项 Software Comparison：

寄存器和驱动程序
• MPU-9250和ICM-20948之间的寄存器地址、位字段定义不同
▪ 虽然功能仍然很常见。
▪ 有关注册地图，请参阅ICM-20948的完整数据表。
• 因此，MPU-9250的驱动程序必须替换为ICM-20948的驱动程序。
 Invense免费提供ICM-20948驱动程序，供原始设备制造商直接使用或作为其自身驱动程序的参考。
 以其中一种方式使用Invense驱动程序可以显著加快迁移速度

驱动代码大小
• Invense驱动程序代码大小已从MPU-9250增加到ICM-20948。
• AP/MCU需要额外容纳约64KB的存储空间。这包括表中DMP代码大小的增加在里面。
• 如果驱动程序使用与功能对应的ICM-20948个性子集，则客户自己的驱动程序可能更小可在MPU-9250中获得。

TDK官网下载驱动代码，程序是针对的官方的开发板代码，但DMP的代码差不多都是一样。
（https://invensense.tdk.com/developers/software-downloads/）

DMP体系结构和功能
▪ ICM-20948的DMP更强大，为AP提供9轴融合数据
▪ MPU-9250只能在其DMP中执行6轴融合
▪ 将融合数据与磁强计原始数据一起传递给AP，以便在AP中进行9轴融合。
▪ 因此，ICM-20948中的DMP具有更高的代码存储内存
▪ 因此，AP/MCU必须容纳容纳DMP图像所需的更高内存

为什么要数据融合？

因为加速度、磁力计具有高频噪声，需要低通滤波，将加速度、磁力计的信号看成是音频信号，它们的信号会有很多“毛刺“，也就是说它们的瞬时值不够精确，解算出来的姿态会震荡，但长期来看姿态方向是对的。

而陀螺仪具有低频噪声，需要高通滤波，即每个时刻的得到的角速度是比较精确的，使用积分就能得到旋转角度（姿态），但是积分会累积误差，因此积分到后面姿态就不对了，也就是漂移现象。 加速度、磁力计和陀螺仪在频域上的特性互补，可以融合这三种传感器的数据，提高精度和系统的动态特性。

DMP与MPL

参考：
1.【CSDN@苏导】DMP和MPL

2.【CSDN@Sky_Lannister】关于六轴DMP和九轴MPL

DMP和MPL的库不一样，InvenSense官方没开源。

DMP

即Digital Motion Processor，是内置在MPU9250中的一个硬件算法单元。其特性是快速，低功耗，可编程控制，内嵌硬件模块。

许多特性支持运行时动态关闭，除了计步数据，其余的DMP数据均输出到MPU9250的FIFO中。

DMP是内置MPU9250的硬件模块，只能拿到Accel和Gyro两个传感器的数据，因此可以从如下的特性列表中可以看出，DMP所支持的所有的算法特性都只是基于Accel和Gyro数据的，其他类型的数据就不能使用DMP了。

MPL

即Motion Processing Library，是Invensense专利的算法库（不开源），其作用是传感器融合和动态标定。

Motion Driver将传感器数据push到MPL lib，然后MPL会处理9个轴的传感器数据的融合。在使能MPL lib前，需要配置MPL特性，这些特性可以调用MPL lib的接口来动态关闭。

九轴MPL刷新的最快频率应该是200Hz，直接出三个欧拉角–偏航、俯仰、翻滚，可以查看四元素，三轴加速度、角速度、磁力值和温度都能读，有单独的读取函数，读出来的都是原始值，还要处理IIC SPI通信方式。

磁力计如果收到的外界磁场过大时，九轴MPL会自动切换为六轴。

也可以不用MPL，手动IMU融合，有算法更直观但是占用运算资源。

• 【CSDN@草帽B-O-Y】IMU姿态融合：从校正到滤波步骤

• 【CSDN@声时刻】IMU加速度、磁力计校正－－椭球拟合

• 【51黑电子论坛】AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析)

总之，有了DMP和MPL算法库 ，就不需要手动做算法去处理数据融合、姿态解算(卡尔曼滤波)等操作了。

ICM20948硬件设计

---

资料汇总：STM32F103&407：MPU6050+MPU9250+ICM20948资料汇总(2022.03.30)

参考资料

• [1] 【B站@正点原子】 手把手教你学STM32 系列视频之 STM32F4-基于探索者F407 P64第65讲 MPU6050六轴传感器实验-M4

• [2] 【正点原子论坛】SPI读取MPU9250 9轴加速度，陀螺仪，磁力计