前言：本文為手把手教學(xué)飛控核心知識(shí)點(diǎn)之一的姿態(tài)解算——MPU6050 姿態(tài)解算（飛控專欄第2篇）。項(xiàng)目中飛行器使用 MPU6050 傳感器對(duì)飛行器的姿態(tài)進(jìn)行解算（四元數(shù)方法），搭配設(shè)計(jì)的卡爾曼濾波器與一階低通濾波器進(jìn)行數(shù)據(jù)濾波。當(dāng)然，本篇博客也將為讀者朋友教學(xué)業(yè)內(nèi)匿名者上位機(jī)的代碼移植和使用方法。為了方便讀者朋友學(xué)習(xí)，本博客將使用傳感器模塊替代整機(jī)進(jìn)行教學(xué)，方便讀者朋友后續(xù)根據(jù)自己實(shí)際情況移植?。?strong>文末有代碼開(kāi)源！）

實(shí)驗(yàn)硬件：?STM32F103C8T6；MPU6050；USB轉(zhuǎn)TTL

硬件實(shí)物圖：

效果圖：

一、飛行器姿態(tài)解算

1.1 MPU6050概述?

飛行器通常搭載一款姿態(tài)傳感器（不管是六軸還是九軸姿態(tài)傳感器），本項(xiàng)目中以最常見(jiàn)的 MPU6050 為例。MPU6050 傳感器其實(shí)并不能直接輸出我們飛行器飛行過(guò)程中的歐拉角(Euler-angles)，通過(guò)讀取它的傳感器我們可以得到：3軸角速度+3軸角加速度。得到的角速度和角加速度信息我們是無(wú)法直接使用的，這個(gè)時(shí)候我們可以選擇使用 DMP 去解算此時(shí)飛行器的歐拉角(Euler-angles)情況。當(dāng)然，作者在項(xiàng)目中并沒(méi)有使用 DMP 去解算飛行器的歐拉角(Euler-angles)，而是使用了四元數(shù)解算的方法！

DMP（Digital Motion Processor）是一種數(shù)字運(yùn)動(dòng)處理器，它可以從MPU6050等傳感器中讀取數(shù)據(jù)，并進(jìn)行解算以獲取姿態(tài)信息。下面是DMP解算MPU6050的優(yōu)缺點(diǎn)：

優(yōu)點(diǎn)：

1. DMP使用簡(jiǎn)單，可以直接套用官方庫(kù)進(jìn)行開(kāi)發(fā)，無(wú)需自己編寫(xiě)解算算法。
2. DMP不會(huì)占用太多的資源，因?yàn)樗恍枰x取傳感器數(shù)據(jù)并進(jìn)行簡(jiǎn)單的解算。
3. DMP的輸出數(shù)據(jù)經(jīng)過(guò)處理，可以直接用于姿態(tài)控制等應(yīng)用，無(wú)需再進(jìn)行復(fù)雜的計(jì)算。

缺點(diǎn)：

1. DMP的輸出數(shù)據(jù)精度可能不夠高，特別是在高精度傳感場(chǎng)景下。
2. DMP的輸出數(shù)據(jù)不穩(wěn)定，可能會(huì)受到噪聲等因素的影響。
3. DMP無(wú)法測(cè)量偏航角，只能獲取滾動(dòng)角和俯仰角的信息。

1.2 四元數(shù)姿態(tài)解算

本小節(jié)將以下方思維導(dǎo)圖進(jìn)行分析講解：

初次接觸的讀者朋友可能對(duì)四元數(shù)較為陌生，這里作者建議大家直接去閱讀秦永元的《慣性導(dǎo)航》，里面有非常好的講解，大家可以直接看緒論和第九章就可以。

《慣性導(dǎo)航》PDF地址：慣性導(dǎo)航（第三版） (sciencereading.cn)

下面我們根據(jù)思維導(dǎo)圖用程序來(lái)一步一步實(shí)現(xiàn)如何求解歐拉角：

1、定義初始四元數(shù)的值為q0=1,q1=0,q2=0,q3=0。

2、讀取加速度計(jì)值、角速度值，程序定義變量分別為ax、ay、az，gx、gy、gz，將陀螺儀值轉(zhuǎn)為弧度，轉(zhuǎn)換如下：

gx = gx * 0.0174f;        //1度=0.0174弧度
gy = gy * 0.0174f; 
gz = gz * 0.0174f; 

3、對(duì)加速度值進(jìn)行歸一化

//提取等效旋轉(zhuǎn)矩陣中的重力分量 
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);								
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);						  
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);	

//加速度歸一化
NormAcc = 1/sqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
	
//歸一化計(jì)算
Acc.x = pMpu->accX * NormAcc;
Acc.y = pMpu->accY * NormAcc;
Acc.z = pMpu->accZ * NormAcc;

4、提取姿態(tài)矩陣中的重力分量，我們已經(jīng)得到數(shù)學(xué)計(jì)算公式為

//提取等效旋轉(zhuǎn)矩陣中的重力分量 
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);								
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);						  
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);	

5、求姿態(tài)誤差，對(duì)兩向量進(jìn)行叉乘(定義ex、ey、ez為三個(gè)軸誤差元素)，數(shù)學(xué)計(jì)算為：?

//向量差乘得出的值
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);

6、互補(bǔ)濾波，將誤差輸入PID控制器后與陀螺儀測(cè)得的角速度相加，修正角速度值，程序?qū)崿F(xiàn)如下(Kp為互補(bǔ)濾波系數(shù)這里取Kp=0.5，實(shí)際值根據(jù)需要進(jìn)行調(diào)整)：?

//角速度融合加速度積分補(bǔ)償值
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x  +  GyroIntegError.x;//弧度制
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y  +  GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z  +  GyroIntegError.z;

7、解四元數(shù)微分方程，其數(shù)學(xué)計(jì)算如下（初始值q0 = 1,q1 = 0,q2 = 0,q3 = 0,為角速度，為周期時(shí)間）

// 一階龍格庫(kù)塔法, 更新四元數(shù)
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;

NumQ.q0 += q0_t;
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;

8、四元數(shù)歸一化，歸一化方法與加速度歸一化方法一樣；

// 四元數(shù)歸一化
NormQuat = 1/sqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
NumQ.q0 *= NormQuat;
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;	

9、計(jì)算姿態(tài)角，數(shù)學(xué)公式為：

	#ifdef	YAW_GYRO
	*(
	float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA;  //yaw
	#else
	float yaw_G = pMpu->gyroZ * Gyro_G;
	if((yaw_G > 1.0f) || (yaw_G < -1.0f)) //數(shù)據(jù)太小可以認(rèn)為是干擾，不是偏航動(dòng)作
	{
		pAngE->yaw  += yaw_G * dt;		
	}
	#endif
pAngE->pitch  =  asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;						
		
pAngE->roll	= atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA;	//PITCH 	

二、卡爾曼濾波詳解

卡爾曼的本質(zhì)：遞歸式最優(yōu)評(píng)估。

卡爾曼的好處是：①效率最高甚至是最有用的，在系統(tǒng)中能夠快速的消除高速白噪聲；②不會(huì)產(chǎn)生嚴(yán)重滯后；③所需數(shù)據(jù)存儲(chǔ)量較小，便于進(jìn)行實(shí)時(shí)處理；

在飛行器中卡爾曼濾波的高效率性是十分優(yōu)秀的，但是卡爾曼不能抵抗突變干擾，這點(diǎn)在飛行器中一般不會(huì)出現(xiàn)數(shù)據(jù)突變跳變，所以卡爾曼很適合運(yùn)用于四軸飛行器。

飛行器項(xiàng)目中卡爾曼濾波的目標(biāo)對(duì)象是加速度，三軸加速度都是獨(dú)立變量，可以分別獨(dú)立用一維線性卡爾曼進(jìn)行濾波。由于加速度容易受震動(dòng)干擾，它就是一個(gè)很典型的高頻高斯白噪聲（噪聲隨機(jī)，對(duì)稱，符合高斯分布的噪聲），所以加速度用卡爾曼濾波。

角速度不容易受到干擾，就用簡(jiǎn)單的一階低通互補(bǔ)濾波。

飛控中的濾波算法：角加速度（卡爾曼濾波）；角速度（一階互補(bǔ)濾波）；

將上述數(shù)學(xué)公式代碼化后可以得到卡爾曼濾波代碼：

#include "kalman.h"

//一維卡爾曼濾波
void kalmanfiter(struct KalmanFilter *EKF,float input)
{
	EKF->NewP = EKF->LastP + EKF->Q;
	EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);
	EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out);
	EKF->LastP = (1 - EKF->Kg) * EKF->NewP;
}

三、CubeMX配置

1、RCC配置外部高速晶振（精度更高）——HSE；

2、SYS配置：Debug設(shè)置成Serial Wire（否則可能導(dǎo)致芯片自鎖）；

3、TIM1配置：在TIM1的中斷回調(diào)函數(shù)中發(fā)生MPU6050姿態(tài)解算與控制都是，中斷周期：3ms；

4、I2C1配置：配置MCU與MPU6050之間的通訊協(xié)議；

5、UART配置：通過(guò)UART1與匿名上位機(jī)進(jìn)行通訊，顯示飛行器3D姿態(tài)；

6、時(shí)鐘樹(shù)配置

7、工程配置

四、代碼與解析

4.1 MPU6050代碼

mpu6050.h代碼：

mpu6050代碼中核心是通過(guò)I2C通訊讀取寄存器地址為：0X3B?和 0x43?的數(shù)值（分別為角加速度和角速度）。

#ifndef __MPU6050_H
#define __MPU6050_H
 	
#include "stm32f1xx_hal.h"http://用什么系列就是什么									  	  
 
//#define MPU_ACCEL_OFFS_REG		0X06	//accel_offs寄存器,可讀取版本號(hào),寄存器手冊(cè)未提到
//#define MPU_PROD_ID_REG			0X0C	//prod id寄存器,在寄存器手冊(cè)未提到
#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	//加速度計(jì)配置寄存器
#define MPU_MOTION_DET_REG		0X1F	//運(yùn)動(dòng)檢測(cè)閥值設(shè)置寄存器
#define MPU_FIFO_EN_REG			0X23	//FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG		0X24	//IIC主機(jī)控制寄存器
#define MPU_I2CSLV0_ADDR_REG	        0X25	//IIC從機(jī)0器件地址寄存器
#define MPU_I2CSLV0_REG			0X26	//IIC從機(jī)0數(shù)據(jù)地址寄存器
#define MPU_I2CSLV0_CTRL_REG	        0X27	//IIC從機(jī)0控制寄存器
#define MPU_I2CSLV1_ADDR_REG	        0X28	//IIC從機(jī)1器件地址寄存器
#define MPU_I2CSLV1_REG			0X29	//IIC從機(jī)1數(shù)據(jù)地址寄存器
#define MPU_I2CSLV1_CTRL_REG	        0X2A	//IIC從機(jī)1控制寄存器
#define MPU_I2CSLV2_ADDR_REG	        0X2B	//IIC從機(jī)2器件地址寄存器
#define MPU_I2CSLV2_REG			0X2C	//IIC從機(jī)2數(shù)據(jù)地址寄存器
#define MPU_I2CSLV2_CTRL_REG	        0X2D	//IIC從機(jī)2控制寄存器
#define MPU_I2CSLV3_ADDR_REG	        0X2E	//IIC從機(jī)3器件地址寄存器
#define MPU_I2CSLV3_REG			0X2F	//IIC從機(jī)3數(shù)據(jù)地址寄存器
#define MPU_I2CSLV3_CTRL_REG	        0X30	//IIC從機(jī)3控制寄存器
#define MPU_I2CSLV4_ADDR_REG	        0X31	//IIC從機(jī)4器件地址寄存器
#define MPU_I2CSLV4_REG			0X32	//IIC從機(jī)4數(shù)據(jù)地址寄存器
#define MPU_I2CSLV4_DO_REG		0X33	//IIC從機(jī)4寫(xiě)數(shù)據(jù)寄存器
#define MPU_I2CSLV4_CTRL_REG	        0X34	//IIC從機(jī)4控制寄存器
#define MPU_I2CSLV4_DI_REG		0X35	//IIC從機(jī)4讀數(shù)據(jù)寄存器
 
#define MPU_I2CMST_STA_REG		0X36	//IIC主機(jī)狀態(tài)寄存器
#define MPU_INTBP_CFG_REG		0X37	//中斷/旁路設(shè)置寄存器
#define MPU_INT_EN_REG			0X38	//中斷使能寄存器
#define MPU_INT_STA_REG			0X3A	//中斷狀態(tài)寄存器
 
#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從機(jī)0數(shù)據(jù)寄存器
#define MPU_I2CSLV1_DO_REG		0X64	//IIC從機(jī)1數(shù)據(jù)寄存器
#define MPU_I2CSLV2_DO_REG		0X65	//IIC從機(jī)2數(shù)據(jù)寄存器
#define MPU_I2CSLV3_DO_REG		0X66	//IIC從機(jī)3數(shù)據(jù)寄存器
 
#define MPU_I2CMST_DELAY_REG	        0X67	//IIC主機(jī)延時(shí)管理寄存器
#define MPU_SIGPATH_RST_REG		0X68	//信號(hào)通道復(fù)位寄存器
#define MPU_MDETECT_CTRL_REG	        0X69	//運(yùn)動(dòng)檢測(cè)控制寄存器
#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計(jì)數(shù)寄存器高八位
#define MPU_FIFO_CNTL_REG		0X73	//FIFO計(jì)數(shù)寄存器低八位
#define MPU_FIFO_RW_REG			0X74	//FIFO讀寫(xiě)寄存器
#define MPU_DEVICE_ID_REG		0X75	//器件ID寄存器,who am i寄存器

//如果AD0腳(9腳)接地,IIC地址為0X68(不包含最低位).
//如果接V3.3,則IIC地址為0X69(不包含最低位).
#define MPU_ADDR				0X68
 
//因?yàn)镸PU6050的AD0接GND,所以則讀寫(xiě)地址分別為0XD1和0XD0
//            (如果AD0接VCC,則讀寫(xiě)地址分別為0XD3和0XD2)  
#define MPU_READ    0XD1
#define MPU_WRITE   0XD0
 
uint8_t MPU_Init(void); 						//初始化MPU6050
uint8_t MPU_Write_Len(uint8_t reg,uint8_t len,uint8_t *buf);                           //IIC連續(xù)寫(xiě)
uint8_t MPU_Read_Len(uint8_t reg,uint8_t len,uint8_t *buf);                         //IIC連續(xù)讀 
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data);				//IIC寫(xiě)一個(gè)字節(jié)
uint8_t MPU_Read_Byte(uint8_t reg);					//IIC讀一個(gè)字節(jié)
 
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);
 
float 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);

void MpuGetData(void);
 
#endif

mpu6050.c代碼：

#include "mpu6050.h" 
#include "alldata.h"
#include "kalman.h"
#include "stdio.h"
#include "i2c.h"

static volatile int16_t *pMpu = (int16_t *)&MPU6050;
int16_t MpuOffset[6] = {0};		//MPU6050補(bǔ)償數(shù)值

//初始化MPU6050
//返回值:0,成功
//    其他,錯(cuò)誤代碼
uint8_t MPU_Init(void)
{ 
  uint8_t res;
  extern I2C_HandleTypeDef hi2c1;
  HAL_I2C_Init(&hi2c1);
  MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//復(fù)位MPU6050
  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);						//設(shè)置采樣率50Hz
  MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//關(guān)閉所有中斷
  MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式關(guān)閉
  MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//關(guān)閉FIFO
  MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引腳低電平有效
  res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
	printf("\r\nMPU6050:0x%2x\r\n",res);
  if(res==MPU_ADDR)//器件ID正確
  {
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//設(shè)置CLKSEL,PLL X軸為參考
    MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度與陀螺儀都工作
    MPU_Set_Rate(50);						//設(shè)置采樣率為50Hz
  }else 
		return 1;
  return 0;
}
//設(shè)置MPU6050陀螺儀傳感器滿量程范圍
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,設(shè)置成功
//    其他,設(shè)置失敗 
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//設(shè)置陀螺儀滿量程范圍  
}
//設(shè)置MPU6050加速度傳感器滿量程范圍
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,設(shè)置成功
//    其他,設(shè)置失敗 
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//設(shè)置加速度傳感器滿量程范圍  
}
//設(shè)置MPU6050的數(shù)字低通濾波器
//lpf:數(shù)字低通濾波頻率(Hz)
//返回值:0,設(shè)置成功
//    其他,設(shè)置失敗 
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);//設(shè)置數(shù)字低通濾波器  
}
//設(shè)置MPU6050的采樣率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,設(shè)置成功
//    其他,設(shè)置失敗 
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);	//設(shè)置數(shù)字低通濾波器
 	return MPU_Set_LPF(rate/2);	//自動(dòng)設(shè)置LPF為采樣率的一半
}
 
//得到溫度值
//返回值:溫度值(擴(kuò)大了100倍)
float MPU_Get_Temperature(void)
{
  unsigned char  buf[2]; 
  short raw;
  float temp;
  
  MPU_Read_Len(MPU_TEMP_OUTH_REG,2,buf); 
  raw=(buf[0]<<8)| buf[1];  
  temp=(36.53+((double)raw)/340)*100;  
//  temp = (long)((35 + (raw / 340)) * 65536L);
  return temp/100.0f;
}
//得到陀螺儀值(原始值)
//gx,gy,gz:陀螺儀x,y,z軸的原始讀數(shù)(帶符號(hào))
//返回值:0,成功
//    其他,錯(cuò)誤代碼
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;  
	res=MPU_Read_Len(MPU_GYRO_XOUTH_REG,6,buf);
	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軸的原始讀數(shù)(帶符號(hào))
//返回值:0,成功
//    其他,錯(cuò)誤代碼
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;  
	res=MPU_Read_Len(MPU_ACCEL_XOUTH_REG,6,buf);
	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連續(xù)寫(xiě)
uint8_t MPU_Write_Len(uint8_t reg,uint8_t len,uint8_t *buf)
{
  extern I2C_HandleTypeDef hi2c1;
  HAL_I2C_Mem_Write(&hi2c1, MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
  HAL_Delay(100);
  
  return 0;
}
//IIC連續(xù)讀
//addr:器件地址
//reg:要讀取的寄存器地址
//len:要讀取的長(zhǎng)度
//buf:讀取到的數(shù)據(jù)存儲(chǔ)區(qū)
//返回值:0,正常
//    其他,錯(cuò)誤代碼
uint8_t MPU_Read_Len(uint8_t reg,uint8_t len,uint8_t *buf)
{ 
  extern I2C_HandleTypeDef hi2c1;
  HAL_I2C_Mem_Read(&hi2c1, MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
  HAL_Delay(100);
  
  return 0;	
}
//IIC寫(xiě)一個(gè)字節(jié) 
//reg:寄存器地址
//data:數(shù)據(jù)
//返回值:0,正常
//    其他,錯(cuò)誤代碼
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data) 				 
{ 
  extern I2C_HandleTypeDef hi2c1;
  unsigned char W_Data=0;
 
  W_Data = data;
  HAL_I2C_Mem_Write(&hi2c1, MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, &W_Data, 1, 0xfff);
  HAL_Delay(100);
  
  return 0;
}
//IIC讀一個(gè)字節(jié) 
//reg:寄存器地址 
//返回值:讀到的數(shù)據(jù)
uint8_t MPU_Read_Byte(uint8_t reg)
{
  extern I2C_HandleTypeDef hi2c1;
  unsigned char R_Data=0;
  
  HAL_I2C_Mem_Read(&hi2c1, MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, &R_Data, 1, 0xfff);
  HAL_Delay(100);
  
  return R_Data;		
}

/* 讀取MPU6050數(shù)據(jù)并加濾波 */
void MpuGetData(void)
{
	uint8_t i;
    uint8_t buffer[12];
	
	HAL_I2C_Mem_Read(&hi2c1, MPU_READ, 0X3B, I2C_MEMADD_SIZE_8BIT, buffer, 6, 0xfff);				/* 讀取角加速度 */
	HAL_I2C_Mem_Read(&hi2c1, MPU_READ, 0x43, I2C_MEMADD_SIZE_8BIT, &buffer[6], 6, 0xfff);		/* 讀取角速度 */
	
	for(i=0;i<6;i++)
	{
		pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];							/* 將數(shù)據(jù)整為16bit，并減去水平校準(zhǔn)值 */
		if(i < 3)		/* 角加速度卡爾曼濾波 */
		{
			{
				static struct KalmanFilter EKF[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};	
				kalmanfiter(&EKF[i],(float)pMpu[i]);  
				pMpu[i] = (int16_t)EKF[i].Out;
			}
		}
		if(i > 2)		/* 角速度一階互補(bǔ)濾波 */
		{	
			uint8_t k=i-3;
			const float factor = 0.15f;  	
			static float tBuff[3];		

			pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;                
		}
	}	
}

4.2 Kalman和一階互補(bǔ)濾波濾波代碼

kalman.c：

#include "kalman.h"

//一維卡爾曼濾波
void kalmanfiter(struct KalmanFilter *EKF,float input)
{
	EKF->NewP = EKF->LastP + EKF->Q;
	EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);
	EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out);
	EKF->LastP = (1 - EKF->Kg) * EKF->NewP;
}

一階互補(bǔ)濾波：

if(i > 2)		/* 角速度一階互補(bǔ)濾波 */
{	
	uint8_t k=i-3;
	const float factor = 0.15f;  	/* 互補(bǔ)濾波的影響因子 */
	static float tBuff[3];		
	pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;                
}

以上 2 種濾波都存在 mpu6050.c 代碼中進(jìn)行調(diào)用：

這個(gè)適當(dāng)取 P = 0.02 默認(rèn)初始第一比數(shù)據(jù)的協(xié)方差不為 0，也就是不承認(rèn)第一筆數(shù)據(jù)完全準(zhǔn)確。取 Q=0.001，就是認(rèn)為測(cè)量方差的過(guò)程噪聲比較小，實(shí)際上多小也沒(méi)人知道，隨便給個(gè)值優(yōu)化輸出。而 R 比較大，默認(rèn)噪聲都是信號(hào)采集就已經(jīng)產(chǎn)生的了(XYZ 三軸加速度都是一個(gè)樣，所以參數(shù)相同?)。

Q/R 決定著濾波的強(qiáng)度，就是收斂速度。收斂速度越快，越容易對(duì)正確的信號(hào)不信任，導(dǎo)致正常的信號(hào)也被衰減，所以濾波有好處有壞處，適當(dāng)就好。?

現(xiàn)在來(lái)介紹一下角速度的濾波。由于角速度不容易受干擾，又需要保持最快速度反應(yīng)。只要稍微適當(dāng)給個(gè)濾波去除白噪聲。本次采用一階低通濾波。

一階低通數(shù)字濾波器的公式為：
y(n) = q*x(n) + (1-q)*y(n-1)

4.3 IMU代碼（姿態(tài)解算）

這部分代碼其實(shí)就是第一章節(jié)的四元數(shù)姿態(tài)求解，具體推導(dǎo)過(guò)程和代碼思路參考上文，還是建議大家多花點(diǎn)時(shí)間消化消化。

imu.h:

#ifndef __IMU_H
#define	__IMU_H

#include "alldata.h"

#define squa( Sq )        (((float)Sq)*((float)Sq))

extern void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt);
extern void imu_rest(void);

#endif

imu.c:

#include "imu.h"
#include "mpu6050.h"
#include <math.h>

const float M_PI = 3.1415926535;
const float RtA = 57.2957795f;
const float AtR = 0.0174532925f;
const float Gyro_G = 0.03051756f*2;	  	//陀螺儀初始化量程+-2000度每秒于1 / (65536 / 4000) = 0.03051756*2		
const float Gyro_Gr = 0.0005326f*2;     //面計(jì)算度每秒,轉(zhuǎn)換弧度每秒則 2*0.03051756	 * 0.0174533f = 0.0005326*2

static float NormAcc;

/* 四元數(shù)系數(shù) */
typedef volatile struct {
  float q0;
  float q1;
  float q2;
  float q3;
} Quaternion;
Quaternion NumQ = {1, 0, 0, 0};

/* 陀螺儀積分誤差 */
struct V{
	float x;
	float y;
	float z;
};	
volatile struct V GyroIntegError = {0};

/* 四元數(shù)解法初始化 */
void imu_rest(void)
{
	NumQ.q0 =1;
	NumQ.q1 = 0;
	NumQ.q2 = 0;
	NumQ.q3 = 0;	
	GyroIntegError.x = 0;
	GyroIntegError.y = 0;
	GyroIntegError.z = 0;
	Angle.pitch = 0;
	Angle.roll = 0;
}

void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt) 
{		
	volatile struct V Gravity,Acc,Gyro,AccGravity;

	static  float KpDef = 0.5f ;
	static  float KiDef = 0.0001f;
//static  float KiDef = 0.00001f;
	
	float q0_t,q1_t,q2_t,q3_t;
  //float NormAcc;	
	float NormQuat; 
	float HalfTime = dt * 0.5f;

	//提取等效旋轉(zhuǎn)矩陣中的重力分量 
	Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);								
	Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);						  
	Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);	
	// 加速度歸一化
	//printf("accX:%d\r\n",MPU6050.accX);
	NormAcc = 1/sqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
	//printf("NorAcc%f\r\n",NormAcc);
	//	NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
	
  Acc.x = pMpu->accX * NormAcc;
  Acc.y = pMpu->accY * NormAcc;
  Acc.z = pMpu->accZ * NormAcc;
	
 	//向量差乘得出的值
	AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
	AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
	AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
	
	//再做加速度積分補(bǔ)償角速度的補(bǔ)償值
  GyroIntegError.x += AccGravity.x * KiDef;
  GyroIntegError.y += AccGravity.y * KiDef;
  GyroIntegError.z += AccGravity.z * KiDef;
	
	//角速度融合加速度積分補(bǔ)償值
  Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x  +  GyroIntegError.x;//弧度制
  Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y  +  GyroIntegError.y;
  Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z  +  GyroIntegError.z;
	
	// 一階龍格庫(kù)塔法, 更新四元數(shù)
	q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
	q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
	q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
	q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
	
	NumQ.q0 += q0_t;
	NumQ.q1 += q1_t;
	NumQ.q2 += q2_t;
	NumQ.q3 += q3_t;
	// 四元數(shù)歸一化
	NormQuat = 1/sqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
	NumQ.q0 *= NormQuat;
	NumQ.q1 *= NormQuat;
	NumQ.q2 *= NormQuat;
	NumQ.q3 *= NormQuat;	
	
	// 四元數(shù)轉(zhuǎn)歐拉角
	{
		 
			#ifdef	YAW_GYRO
			*(
		float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA;  //yaw
			#else
				float yaw_G = pMpu->gyroZ * Gyro_G;
				if((yaw_G > 1.0f) || (yaw_G < -1.0f)) //數(shù)據(jù)太小可以認(rèn)為是干擾，不是偏航動(dòng)作
				{
					pAngE->yaw  += yaw_G * dt;
					printf("Yaw:%f\r\n",pAngE->yaw);
				}
			#endif
			pAngE->pitch  =  asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;						
		
			pAngE->roll	= atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA;	//PITCH 			
			printf("Pitch:%f;\r\n",pAngE->pitch);
			printf("Roll:%f;\r\n",pAngE->roll);
	}
}

4.4 main函數(shù)

該階段下的 main 函數(shù)為 while 空循環(huán)，MPU6050 讀取和解算歐拉角都在 TIM1 的中斷回調(diào)函數(shù)中進(jìn)行，代碼如下：

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == (&htim1))	//如果定時(shí)器TIM1中斷發(fā)生
    {
			MpuGetData();
			GetAngle(&MPU6050,&Angle,0.003f);
			Send_ANOTC();
		}
}

五、匿名上位機(jī)3D姿態(tài)顯示

5.1 匿名上位機(jī)概述

匿名上位機(jī)為匿名科創(chuàng)團(tuán)隊(duì)自主設(shè)計(jì)的知名上位機(jī)，通常用于與無(wú)人機(jī)或飛行器進(jìn)行通信和數(shù)據(jù)傳輸。它可以通過(guò)串口、藍(lán)牙、Wi-Fi和USB等通信方式與無(wú)人機(jī)連接，并可以實(shí)時(shí)接收和顯示來(lái)自無(wú)人機(jī)的數(shù)據(jù)，如傳感器數(shù)據(jù)、GPS定位信息、飛行控制指令等。此外，匿名上位機(jī)還可以進(jìn)行數(shù)據(jù)存儲(chǔ)、分析和可視化3D模型，方便用戶對(duì)無(wú)人機(jī)數(shù)據(jù)進(jìn)行處理和分析（作者本次僅為大家提供串口協(xié)議下的可視化3D姿態(tài)代碼）。

匿名科創(chuàng)官網(wǎng)：welcome [匿名科創(chuàng)] (anotc.com)

5.2 匿名上位機(jī)通訊

補(bǔ)充說(shuō)明：匿名科創(chuàng)團(tuán)隊(duì)制作的匿名上位機(jī)一直在迭代升級(jí)，不同版本之間的通訊協(xié)議和功能使用時(shí)可能存在一定差異。作者使用的匿名上位機(jī)V5.0版本（上位機(jī)軟件在文末資源包內(nèi)！）

匿名科創(chuàng)上位機(jī)為使用者提供了詳細(xì)的通訊協(xié)議資料，點(diǎn)擊左邊程序設(shè)置圖標(biāo)，點(diǎn)擊紅框所示的通信協(xié)議。

匿名上位機(jī)提供了很多數(shù)據(jù)傳輸協(xié)議模板，此時(shí)飛控項(xiàng)目我們僅考慮飛機(jī)姿態(tài)的基本信息，從WPS的表格中找到如下信息：

通過(guò)上圖可以清晰的發(fā)現(xiàn)，匿名上位機(jī)的通訊是使用數(shù)據(jù)幀格式的（即存在規(guī)定格式的數(shù)據(jù)格式）。接下來(lái)作者幫大家解析一下STATUS幀下的數(shù)據(jù)格式（飛機(jī)姿態(tài)信息幀）。

為了方便讀者朋友對(duì)數(shù)據(jù)格式的解讀，作者這里將匿名上位機(jī)的數(shù)據(jù)幀分為4部分：

第一部分：幀頭“AAAA”，匿名上位機(jī)每次接受數(shù)據(jù)都需要“AAAA”格式的幀頭打前陣；

第二部分：功能字+長(zhǎng)度部分，STATUS幀下的功能字為0x01；▲協(xié)議中長(zhǎng)度字節(jié)LEN表示該數(shù)據(jù)幀內(nèi)包含數(shù)據(jù)的字節(jié)總長(zhǎng)度，不包括幀頭、功能字、長(zhǎng)度字節(jié)和最后的校驗(yàn)位，只是數(shù)據(jù)的字節(jié)長(zhǎng)度和。比如該幀數(shù)據(jù)內(nèi)容為3個(gè)int16型數(shù)據(jù)，那么LEN等于6，一個(gè)字節(jié)（8位）算一個(gè)長(zhǎng)度。故STATUS幀下的數(shù)據(jù)長(zhǎng)度為：2+2+2+4+1+1=12；

第三部分：核心數(shù)據(jù)部分，該部分為通訊協(xié)議中的核心部分。定義一個(gè)數(shù)組，寫(xiě)入：翻滾角roll*100，俯仰角pitch*100，偏航角yaw*100，0（為使用氣壓計(jì)），0（飛行模式），0（加鎖）；

第四部分：SUM即校驗(yàn)位，▲SUM等于從該數(shù)據(jù)幀第一字節(jié)開(kāi)始，也就是幀頭開(kāi)始，至該幀數(shù)據(jù)的最后一字節(jié)所有字節(jié)的和，只保留低八位，高位舍去。當(dāng)然，校驗(yàn)位SUM并非必須的，你可以不寫(xiě)；

貼心寄語(yǔ)：在實(shí)際嵌入式工程項(xiàng)目中，對(duì)于數(shù)據(jù)幀的處理是非常常見(jiàn)的。很多時(shí)候往往需要工程師自己制定和編寫(xiě)收發(fā)數(shù)據(jù)幀格式，作者建議大家可以多磨練這方面的能力。

作者通過(guò)串口usart1與匿名上位機(jī)進(jìn)行通訊，通訊協(xié)議代碼如下，供大家參考：

comm.c代碼：

#include "comm.h"
#include "alldata.h"
#include "usart.h"

/* ANOTC匿名站通訊機(jī)制（串口版本） */
void Send_ANOTC()
{
	uint8_t i;
	uint8_t len=0;							
	int16_t Anto[12];
	int8_t *pt = (int8_t*)(Anto);		
	
	Anto[2] = (int16_t)(Angle.roll*100);
	Anto[3] = (int16_t)(Angle.pitch*100);
	Anto[4] = -(int16_t)(Angle.yaw*100);
	Anto[5] = 0;//沒(méi)有高度數(shù)據(jù)
	Anto[6] = 0;//飛行模式
	Anto[7] = ALL_flag.unlock<<8;//解鎖信息
	len = 12;						//數(shù)據(jù)長(zhǎng)度
	
	Anto[0] = 0XAAAA;            //幀頭
	Anto[1] = len | 0x01<<8;     //功能字+長(zhǎng)度
	pt[len+4] = (int8_t)(0xAA+0xAA);
	
	pt[len+4] = (int8_t)(0xAA+0xAA);
	for(i=2;i<len+4;i+=2)    //a swap with b;
	{
		pt[i] ^= pt[i+1];
		pt[i+1] ^= pt[i];
		pt[i] ^= pt[i+1];
		pt[len+4] += pt[i] + pt[i+1];
	}
	
	HAL_UART_Transmit(&huart1,pt,len+5,0xFFFF);    //采用串口發(fā)送到匿名上位機(jī)
}

5.3 匿名上位機(jī)使用

匿名上位機(jī)V5.0版本提供了3種類型的通訊方式，我們這里選擇串口模式，波特率設(shè)置為115200，具體如下圖所示：

第一步：點(diǎn)擊右下角打開(kāi)連接；第二步：點(diǎn)擊飛控狀態(tài)圖標(biāo)；（作者沒(méi)有寫(xiě)數(shù)據(jù)校驗(yàn)位故右下角警告可能出現(xiàn)警告，無(wú)視即可！）

我們僅需要關(guān)注ROL、RIT、YAW和飛控狀態(tài)即可，可以看出來(lái)匿名上位機(jī)的使用是非常簡(jiǎn)單！

六、項(xiàng)目效果

七、項(xiàng)目代碼

代碼地址：?基于STM32的四旋翼無(wú)人機(jī)項(xiàng)目（二）：MPU6050姿態(tài)解算（含匿名上位機(jī)串口通訊版本代碼）-嵌入式文檔類資源-CSDN文庫(kù)https://download.csdn.net/download/black_sneak/87934739

如果積分不夠的朋友，點(diǎn)波關(guān)注，評(píng)論區(qū)留下郵箱，作者無(wú)償提供源碼和后續(xù)問(wèn)題解答。求求啦關(guān)注一波吧?！??！文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-495370.html

到了這里，關(guān)于基于STM32的四旋翼無(wú)人機(jī)項(xiàng)目（二）：MPU6050姿態(tài)解算（含上位機(jī)3D姿態(tài)顯示教學(xué)）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！