一. 緒論
二. JY901B與JY-GPSIMU角度傳感器介紹
- 1. 角度傳感器簡介
- 2. JY901B的IIC通訊協議
- 3. JY-GPSIMU的串口通訊協議
三. STM32的IIC與串口讀取三軸角度驅動程式
- 1. IIC讀取JY901B角度傳感器的角度
- 2. UART讀取JY-GPSIMU角度傳感器的角度
四. 傳感任務應用程式
- 1. 姿態角滑動平均濾波
- 2. 傳感任務創建
- 3. 傳感任務中應用程式

一. 緒論

上一篇STM32實作水下四旋翼（四）傳感任務1——姿態解算原理篇中我們一起復習了傳感器測量原理與狀態估計的理論知識，這些內容非常非常重要，但很遺憾的是在本系列文章中沒有用到，，因為我使用的是角度傳感器，通過串口直接讀取三軸角度，至于傳感器內部，它就是使用的卡爾曼濾波進行角度估計的，所以我們省略了一步驟，如果是使用MPU6050、MPU9050這類傳感器，那么就需要使用互補濾波或卡爾曼濾波演算法進行姿態解算了，

長文預警！！！因為代碼確實太多了，請耐心看下去，想學東西的一定不會失望哈！

二. JY901B與JY-GPSIMU角度傳感器介紹

1. 角度傳感器簡介

水下四旋翼使用了兩個角度傳感器——維特智能的JY901B和十軸慣導JY-GPSIMU，價格分別為100多和500多，如圖所示（宣告，不是打廣告，你上淘寶搜角度傳感器基本都是他們家的，使用確實很方便），這兩個器件都能測氣壓、高度，十軸慣導還帶GPS ，使用兩個傳感器是冗余設計，有的要求高的場合用三個四個的都有，至于怎么去使用多個，是只用一個還是多個資料取平均都可以靈活調整，

JY901B的資料輸出是IIC和UART兩種方式，十軸慣導只有UART輸出，本文分別用IIC和UART2讀兩個器件的角度資料，另外JY901B最好焊在板子上，或者用引腳對插，總之注意慣性器件安裝一定要穩固，

（使用JY901B是因為一開始設計板子的時候計劃將其焊在板子上，后來也是這么干的，但方便起見建議全部使用外接的角度傳感器），

使用傳感器當然要了解它的通信介面和通信協議啦，下面我們分別來看一下兩個器件的通信協議，后面寫驅動代碼就以此為依據了，這兩個器件由于是一家公司產的，模塊內部暫存器地址和通信協議是通用的，可以通過上位機軟體設定不同模式、更改配置、校準等，也可以通過串口或IIC通訊寫入命令進行上述操作，暫存器地址如下:
注意我標黃的部分就是我們讀慣導資料的地址，

2. JY901B的IIC通訊協議

（1）寫資料

IIC 寫入的時序資料格式如下：


IICAddr<<1	RegAddr	Data1L	Data1H	Data2L	Data2H	……

首先單片機向 JY-901 模塊發送一個 Start 信號，在將模塊的 IIC 地址 IICAddr 寫入，在寫入暫存器地址 RegAddr，在順序寫入第一個資料的低位元組，第一個資料的高位元組，如果還有資料，可以繼續按照先低位元組后高位元組的順序寫入，當最后一個資料寫完以后，主機向模塊發送一個停止信號，讓出 IIC 總線，

當高位元組資料傳入 JY-901 模塊以后，模塊內部的暫存器將更新并執行相應的指令，同時模塊內部的暫存器地址自動加 1，地址指標指向下一個需要寫入的暫存器地址，這樣可以實作連續寫入，

這個寫入時序是通用的IIC寫入時序，需要注意的就是暫存器的地址，比如設定模塊IIC通信地址為0x55,則RegAddr 為 0x1a（查表得）， DataL 為 0x55， DataH 為0x00，

（2）讀資料

IIC 寫入的時序資料格式如下：


IICAddr<<1	RegAddr	(IICAddr<<1)/1	Data1L	Data1H	Data2L	Data2H	……

ps: 上面是(IICAddr<<1)|1,因為打|符號會與Markdown語法沖突，，，

首先單片機向 JY-901 模塊發送一個 Start 信號，在將模塊的 IIC 地址 IICAddr 寫入，在寫入暫存器地址 RegAddr，主機再向模塊發送一個讀信號(IICAddr<<1)|1，此后模塊將按照先低位元組，后高位元組的順序輸出資料，主機需在收到每一個位元組后，拉低 SDA 總線，向模塊發出一個應答信號，待接收完指定數量的資料以后，主機不再向模塊回饋應答信號，此后模塊將不再輸出資料，主機向模塊再發送一個停止信號，以結束本次操作，

這個寫入時序也是通用的IIC寫入時序，需要注意的就是暫存器的地址，比如讀取模塊的角度資料，則RegAddr 為 0x3d， 0x3e， 0x3f（查表得），連續讀取六個位元組即可，

3. JY-GPSIMU的串口通訊協議

（1）串口寫入

資料格式：


0xFF	0xAA	Address	DataL	DataH

比如設定串口波特率為115200（對應Data為0x06）


0xFF	0xAA	0x04	0x06	0x00

（2）串口讀取

串口讀取是模塊按一定的回傳速度定時上傳資料的，資料幀格式為每幀11位元組，


0x55	ID	Data1	Data2	Data3	Data4	Data5	Data6	Data7	Data8

Sum=0x55+ID+Data1+…+Data8

我們只關注幾個重要的資料：

加速度輸出：


0x55	0x51	AxL	AxH	AyL	AyH	AzL	AzH	TL	TH

分別為三軸的加速度和溫度高低位元組，

角速度輸出：


0x55	0x52	wxL	wxH	wyL	wyH	wzL	wzH	TL	TH

分別為三軸的角速度和溫度高低位元組，

角度輸出：


0x55	0x53	AnglexL	AnglexH	AngleyL	AngleyH	AnglezL	AnglezH	TL	TH

分別為三軸的角度和溫度高低位元組，

磁場輸出：


0x55	0x52	MxL	MxH	MyL	MyH	MzL	MzH	TL	TH

分別為三軸的磁場強度和溫度高低位元組，

其他還有氣壓高度輸出、經緯度輸出、四元數輸出、GPS資料輸出等就不列舉了，

另外無論是IIC讀取還是串口上傳，都是原始資料，需要經過公式換算得到實際值，換算公式傳感器說明書有給，后面的程式里面也會看到，

三. STM32的IIC與串口讀取三軸角度驅動程式

1. IIC讀取JY901B角度傳感器的角度

創建JY901_IIC.h和JY901_IIC.c兩個檔案，用于IIC驅動，下面把這兩個檔案的內容附上，使用的是軟體模擬IIC（這是通用的IIC的驅動，你在其他地方肯定也看到過）

JY901_IIC.h內容如下，實作函式宣告和宏定義

#ifndef _JY901_IIC_H
#define _JY901_IIC_H
#include "sys.h"

//IO方向設定
#define SDA_IN()  {GPIOH->MODER&=~(3<<(4*2));GPIOH->MODER|=0<<4*2;}	//PH3輸入模式
#define SDA_OUT() {GPIOH->MODER&=~(3<<(4*2));GPIOH->MODER|=1<<4*2;} //PH3輸出模式
//IO操作
#define IIC_SCL(n)  (n?HAL_GPIO_WritePin(GPIOH,GPIO_PIN_5,GPIO_PIN_SET):HAL_GPIO_WritePin(GPIOH,GPIO_PIN_5,GPIO_PIN_RESET)) //SCL
#define IIC_SDA(n)  (n?HAL_GPIO_WritePin(GPIOH,GPIO_PIN_4,GPIO_PIN_SET):HAL_GPIO_WritePin(GPIOH,GPIO_PIN_4,GPIO_PIN_RESET)) //SDA
#define READ_SDA    HAL_GPIO_ReadPin(GPIOH,GPIO_PIN_4)  //輸入SDA

//IIC所有操作函式
void IIC_Init(void);                //初始化IIC的IO口				 
void IIC_Start(void);				//發送IIC開始信號
void IIC_Stop(void);	  			//發送IIC停止信號
void IIC_Send_Byte(u8 txd);			//IIC發送一個位元組
u8 IIC_Read_Byte(unsigned char ack);//IIC讀取一個位元組
u8 IIC_Wait_Ack(void); 				//IIC等待ACK信號
void IIC_Ack(void);					//IIC發送ACK信號
void IIC_NAck(void);				//IIC不發送ACK信號

void IIC_Write_One_Byte(u8 daddr,u8 addr,u8 data);
u8 IIC_Read_One_Byte(u8 daddr,u8 addr);	 

unsigned char I2C_ReadOneByte(unsigned char I2C_Addr,unsigned char addr);
unsigned char IICwriteByte(unsigned char dev, unsigned char reg, unsigned char data);
unsigned char IICwriteCmd(unsigned char dev, unsigned char cmd);
u8 IICwriteBytes(u8 dev, u8 reg, u8 length, u8* data);
u8 IICwriteBits(u8 dev,u8 reg,u8 bitStart,u8 length,u8 data);
u8 IICwriteBit(u8 dev,u8 reg,u8 bitNum,u8 data);
u8 IICreadBytes(u8 dev, u8 reg, u8 length, u8 *data);

void ShortToChar(short sData,unsigned char cData[]);
short CharToShort(unsigned char cData[]);

#endif

JY901_IIC.c中實作硬體初始化、函式定義

#include "JY901_IIC.h"
#include "delay.h"

//IIC初始化
void IIC_Init(void)
{
	GPIO_InitTypeDef GPIO_Initure;
	__HAL_RCC_GPIOH_CLK_ENABLE(); //使能GPIOH時鐘
	//PH4,5初始化設定
	GPIO_Initure.Pin = GPIO_PIN_4 | GPIO_PIN_5;
	GPIO_Initure.Mode = GPIO_MODE_OUTPUT_PP; //推挽輸出
	GPIO_Initure.Pull = GPIO_PULLUP;		 //上拉
	GPIO_Initure.Speed = GPIO_SPEED_FAST;	 //快速
	HAL_GPIO_Init(GPIOH, &GPIO_Initure);

	IIC_SDA(1);
	IIC_SCL(1);
}

//產生IIC起始信號
void IIC_Start(void)
{
	SDA_OUT(); //sda線輸出
	IIC_SDA(1);
	IIC_SCL(1);
	delay_us(5);
	IIC_SDA(0); //START:when CLK is high,DATA change form high to low
	delay_us(5);
	IIC_SCL(0); //鉗住I2C總線，準備發送或接收資料
}
//產生IIC停止信號
void IIC_Stop(void)
{
	SDA_OUT(); //sda線輸出
	IIC_SCL(0);
	IIC_SDA(0); //STOP:when CLK is high DATA change form low to high
	delay_us(5);
	IIC_SCL(1);
	delay_us(5);
	IIC_SDA(1); //發送I2C總線結束信號
}
//等待應答信號到來
//回傳值：1，接收應答失敗
//        0，接收應答成功
u8 IIC_Wait_Ack(void)
{
	u8 ucErrTime = 0;
	SDA_IN(); //SDA設定為輸入
	IIC_SDA(1);
	delay_us(5);
	IIC_SCL(1);
	delay_us(5);
	while (READ_SDA)
	{
		ucErrTime++;
		if (ucErrTime > 250)
		{
			IIC_Stop();
			return 1;
		}
	}
	IIC_SCL(0); //時鐘輸出0
	return 0;
}
//產生ACK應答
void IIC_Ack(void)
{
	IIC_SCL(0);
	SDA_OUT();
	IIC_SDA(0);
	delay_us(5);
	IIC_SCL(1);
	delay_us(5);
	IIC_SCL(0);
}
//不產生ACK應答
void IIC_NAck(void)
{
	IIC_SCL(0);
	SDA_OUT();
	IIC_SDA(1);
	delay_us(5);
	IIC_SCL(1);
	delay_us(5);
	IIC_SCL(0);
}
//IIC發送一個位元組
//回傳從機有無應答
//1，有應答
//0，無應答
void IIC_Send_Byte(u8 txd)
{
	u8 t;
	SDA_OUT();
	IIC_SCL(0); //拉低時鐘開始資料傳輸
	for (t = 0; t < 8; t++)
	{
		IIC_SDA((txd & 0x80) >> 7);
		txd <<= 1;
		delay_us(2); //對TEA5767這三個延時都是必須的
		IIC_SCL(1);
		delay_us(5);
		IIC_SCL(0);
		delay_us(3);
	}
}
//讀1個位元組，ack=1時，發送ACK，ack=0，發送nACK
u8 IIC_Read_Byte(unsigned char ack)
{
	unsigned char i, receive = 0;
	SDA_IN(); //SDA設定為輸入
	for (i = 0; i < 8; i++)
	{
		IIC_SCL(0);
		delay_us(5);
		IIC_SCL(1);
		receive <<= 1;
		if (READ_SDA)
			receive++;
		delay_us(5);
	}
	if (!ack)
		IIC_NAck(); //發送nACK
	else
		IIC_Ack(); //發送ACK
	return receive;
}

/**************************實作函式********************************************
*函式原型:		u8 IICreadBytes(u8 dev, u8 reg, u8 length, u8 *data)
*功　　能:	    讀取指定設備 指定暫存器的 length個值
輸入	dev  目標設備地址
		reg	  暫存器地址
		length 要讀的位元組數
		*data  讀出的資料將要存放的指標
回傳   讀出來的位元組數量
*******************************************************************************/
u8 IICreadBytes(u8 dev, u8 reg, u8 length, u8 *data)
{
	u8 count = 0;

	IIC_Start();
	IIC_Send_Byte(dev << 1); //發送寫命令
	IIC_Wait_Ack();
	IIC_Send_Byte(reg); //發送地址
	IIC_Wait_Ack();
	IIC_Start();
	IIC_Send_Byte((dev << 1) + 1); //進入接收模式
	IIC_Wait_Ack();

	for (count = 0; count < length; count++)
	{

		if (count != length - 1)
			data[count] = IIC_Read_Byte(1); //帶ACK的讀資料
		else
			data[count] = IIC_Read_Byte(0); //最后一個位元組NACK
	}
	IIC_Stop(); //產生一個停止條件
	return count;
}

/**************************實作函式********************************************
*函式原型:		u8 IICwriteBytes(u8 dev, u8 reg, u8 length, u8* data)
*功　　能:	    將多個位元組寫入指定設備 指定暫存器
輸入	dev  目標設備地址
		reg	  暫存器地址
		length 要寫的位元組數
		*data  將要寫的資料的首地址
回傳   回傳是否成功
*******************************************************************************/
u8 IICwriteBytes(u8 dev, u8 reg, u8 length, u8 *data)
{

	u8 count = 0;
	IIC_Start();
	IIC_Send_Byte(dev << 1); //發送寫命令
	IIC_Wait_Ack();
	IIC_Send_Byte(reg); //發送地址
	IIC_Wait_Ack();
	for (count = 0; count < length; count++)
	{
		IIC_Send_Byte(data[count]);
		IIC_Wait_Ack();
	}
	IIC_Stop(); //產生一個停止條件

	return 1; //status == 0;
}

void ShortToChar(short sData, unsigned char cData[])
{
	cData[0] = sData & 0xff;
	cData[1] = sData >> 8;
}
short CharToShort(unsigned char cData[])
{
	return ((short)cData[1] << 8) | cData[0];
}

另外創建一個JY901_REG檔案（廠家例程自帶），按照上面的暫存器地址表進行暫存器宏定義（只顯示了本文用到的）：

#ifndef __JY901_REG_H
#define __JY901_REG_H

#define AX					0x34
#define AY					0x35
#define AZ					0x36
#define GX					0x37
#define GY					0x38
#define GZ					0x39
#define HX					0x3a
#define HY					0x3b
#define HZ					0x3c			
#define Roll				0x3d
#define Pitch				0x3e
#define Yaw					0x3f
#define TEMP				0x40
#define PressureL		    0x45
#define PressureH		    0x46
#define HeightL			    0x47
#define HeightH			    0x48
#define LonL				0x49
#define LonH				0x4a
#define LatL				0x4b
#define LatH				0x4c
#define GPSHeight   		0x4d
#define GPSYAW    		    0x4e
#define GPSVL				0x4f
#define GPSVH				0x50

#endif

創建一個sensor.c和sensor.h檔案，所有的讀傳感器資料的代碼都寫在這個檔案中，sensor.c中添加如下函式：

void sensor_Init(void)
{
    IIC_Init();			// JY901
    MS5837_init();		// 水身傳感器
}

void IIC_ReadJY901(float *Gyro, float *Acc, float *Mag, float *Angle)
{
	OS_ERR err;
	CPU_SR_ALLOC();
    unsigned char chrTemp[30];
	OS_CRITICAL_ENTER();
    IICreadBytes(0x50, AX, 24,&chrTemp[0]);
	OS_CRITICAL_EXIT();
	
    //加速度值
    Acc[0] = (float)CharToShort(&chrTemp[0])/32768*16;
    Acc[1] = (float)CharToShort(&chrTemp[2])/32768*16;
    Acc[2] = (float)CharToShort(&chrTemp[4])/32768*16;
    //角速度值
    Gyro[0] = (float)CharToShort(&chrTemp[6])/32768*2000;
    Gyro[1] = (float)CharToShort(&chrTemp[8])/32768*2000;
    Gyro[2] = (float)CharToShort(&chrTemp[10])/32768*2000;
    //磁力計值
    Mag[0] = CharToShort(&chrTemp[12]);
    Mag[1] = CharToShort(&chrTemp[14]);
    Mag[2] = CharToShort(&chrTemp[16]);
    //姿態角
    Angle[0] = (float)CharToShort(&chrTemp[18])/32768*180;
    Angle[1] = (float)CharToShort(&chrTemp[20])/32768*180;
    Angle[2] = (float)CharToShort(&chrTemp[22])/32768*180;
}

至于sensor.h檔案里面都是對sensor.c中函式的宣告，方便呼叫，這里就不貼了，

到這里就可以通過呼叫void IIC_ReadJY901(float *Gyro, float *Acc, float *Mag, float *Angle)讀取一次三軸角度、角速度、加速度值了，不過這還不夠，還需要濾下波，請往下看，

2. UART讀取JY-GPSIMU角度傳感器的角度

串口讀取資料就很簡單了，按照通信協議去決議就行了，這里參考廠家官方例程，創建一個HT905.c和HT905.h檔案，HT905.h中宣告一些結構體型別和相應的結構體變數，相應的結構體變數的定義放在HT905.c中（宏定義、結構體型別的定義、變數宣告、函式宣告在頭檔案，變數的定義、函式的定義在c檔案，應該沒有疑義哈）：

#ifndef __HT905_H
#define __HT905_H
#include "sys.h"
struct STime
{
	unsigned char ucYear;
	unsigned char ucMonth;
	unsigned char ucDay;
	unsigned char ucHour;
	unsigned char ucMinute;
	unsigned char ucSecond;
	unsigned short usMiliSecond;
};
struct SAcc
{
	short a[3];
	short T;
};
struct SGyro
{
	short w[3];
	short T;
};
struct SAngle
{
	short Angle[3];
	short T;
};
struct SMag
{
	short h[3];
	short T;
};

struct SDStatus
{
	short sDStatus[4];
};

struct SPress
{
	long lPressure;
	long lAltitude;
};

struct SLonLat
{
	long lLon;
	long lLat;
};

struct SGPSV
{
	short sGPSHeight;
	short sGPSYaw;
	long lGPSVelocity;
};

struct SQuat
{
	short q[4];
};

extern struct STime		stcTime;
extern struct SAcc 		stcAcc;
extern struct SGyro 		stcGyro;
extern struct SAngle 	stcAngle;
extern struct SMag 		stcMag;
extern struct SDStatus stcDStatus;
extern struct SPress 	stcPress;
extern struct SLonLat 	stcLonLat;
extern struct SGPSV 		stcGPSV;
extern struct SQuat 		stcQuat;

#endif

HT905.c的檔案內容如下，就是定義了幾個結構體變數，分別是不同的資料（時間、加速度、角速度、角度等）：

#include "HT905.h"
#include "usart.h"
#include "delay.h"

struct STime		stcTime;
struct SAcc 		stcAcc;
struct SGyro 		stcGyro;
struct SAngle 	stcAngle;
struct SMag 		stcMag;
struct SDStatus stcDStatus;
struct SPress 	stcPress;
struct SLonLat 	stcLonLat;
struct SGPSV 		stcGPSV;
struct SQuat 		stcQuat;

回到之前的uart.c和uart.h檔案，之前我們在里面寫了串口1的驅動程式，因為要使用串口2讀角度，現在我們繼續添加串口2的驅動程式（下面直接把四個串口的都加上了，后面就不重復寫了），

#ifndef _USART_H
#define _USART_H
#include "sys.h"
#include "stdio.h"	
#include "pid.h"
 	
#define USART_REC_LEN  			100  	//定義最大接收位元組數 200
#define RXBUFFERSIZE   1 //快取大小

//串口1
#define EN_USART1_RX 			1		//使能（1）/禁止（0）串口1接收  	
extern u8  USART1_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.末位元組為換行符 
extern u16 USART1_RX_STA;         		//接收狀態標記	
extern UART_HandleTypeDef UART1_Handler; //UART句柄
extern u8 aRxBuffer1[RXBUFFERSIZE];//HAL庫USART接收Buffer

//串口2
#define EN_USART2_RX 			1		//使能（1）/禁止（0）串口1接收
extern u8  USART2_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.末位元組為換行符 
extern u16 USART2_RX_STA;         		//接收狀態標記	
extern UART_HandleTypeDef UART2_Handler; //UART句柄
extern u8 aRxBuffer2[RXBUFFERSIZE];//HAL庫USART接收Buffer

//串口3
#define EN_USART3_RX 			1		//使能（1）/禁止（0）串口1接收
extern u8  USART3_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.末位元組為換行符 
extern u16 USART3_RX_STA;         		//接收狀態標記	
extern UART_HandleTypeDef UART3_Handler; //UART句柄
extern u8 aRxBuffer3[RXBUFFERSIZE];//HAL庫USART接收Buffer

//串口4
#define EN_USART4_RX 			1		//使能（1）/禁止（0）串口1接收
extern u8  USART4_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.末位元組為換行符 
extern u16 USART4_RX_STA;         		//接收狀態標記	
extern UART_HandleTypeDef UART4_Handler; //UART句柄
extern u8 aRxBuffer4[RXBUFFERSIZE];     //HAL庫USART接收Buffer

void uart1_init(u32 bound);
void uart2_init(u32 bound);
void uart3_init(u32 bound);
void uart4_init(u32 bound);

void CopeSerial2Data(unsigned char ucData);

#endif

uart.c更新如下：

#include "usart.h"
#include "sys.h"
#include <iwdg.h>
#include "pid.h"
#include "HT905.h"
#include "string.h"
#include "sbus.h"
#include "transmission.h"
//
//如果使用os,則包括下面的頭檔案即可.
#if SYSTEM_SUPPORT_OS
#include "includes.h" //os 使用
#endif

#if 1
#pragma import(__use_no_semihosting)
//標準庫需要的支持函式
struct __FILE
{
	int handle;
};

FILE __stdout;
//定義_sys_exit()以避免使用半主機模式
void _sys_exit(int x)
{
	x = x;
}

int fputc(int ch, FILE *f)
{
	while ((USART2->ISR & 0X40) == 0)
		; //回圈發送,直到發送完畢
	USART2->TDR = (u8)ch;
	return ch;
}
#endif

//串口1中斷服務程式
u8 USART1_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.
u16 USART1_RX_STA = 0; //接收狀態標記
u8 aRxBuffer1[RXBUFFERSIZE];		  //HAL庫使用的串口接識訓沖
UART_HandleTypeDef UART1_Handler; //UART句柄

//串口2中斷服務程式
u8 USART2_RX_BUF[USART_REC_LEN];     //接識訓沖,最大USART_REC_LEN個位元組.
u16 USART2_RX_STA=0;       //接收狀態標記	
u8 aRxBuffer2[RXBUFFERSIZE];//HAL庫使用的串口接識訓沖
UART_HandleTypeDef UART2_Handler; //UART句柄

//串口3
u8  USART3_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.末位元組為換行符 
u16 USART3_RX_STA;         		//接收狀態標記	
UART_HandleTypeDef UART3_Handler; //UART句柄
u8 aRxBuffer3[RXBUFFERSIZE];//HAL庫USART接收Buffer

//串口4
u8  USART4_RX_BUF[USART_REC_LEN]; //接識訓沖,最大USART_REC_LEN個位元組.末位元組為換行符 
u16 USART4_RX_STA;         		//接收狀態標記	
UART_HandleTypeDef UART4_Handler; //UART句柄
u8 aRxBuffer4[RXBUFFERSIZE];//HAL庫USART接收Buffer

//初始化IO 串口1
//bound:波特率
void uart1_init(u32 bound)
{
	//UART 初始化設定
	UART1_Handler.Instance = USART1;					//USART1
	UART1_Handler.Init.BaudRate = bound;				//波特率
	UART1_Handler.Init.WordLength = UART_WORDLENGTH_9B; //字長為8位資料格式
	UART1_Handler.Init.StopBits = UART_STOPBITS_1;		//一個停止位
	UART1_Handler.Init.Parity = UART_PARITY_EVEN;		//無奇偶校驗位
	UART1_Handler.Init.HwFlowCtl = UART_HWCONTROL_NONE; //無硬體流控
	UART1_Handler.Init.Mode = UART_MODE_TX_RX;			//收發模式
	HAL_UART_Init(&UART1_Handler);						//HAL_UART_Init()會使能UART1

	HAL_UART_Receive_IT(&UART1_Handler, (u8 *)aRxBuffer1, RXBUFFERSIZE); //該函式會開啟接收中斷：標志位UART_IT_RXNE，并且設定接識訓沖以及接識訓沖接收最大資料量
}

void uart2_init(u32 bound)
{
	//UART3 初始化設定
	UART2_Handler.Instance=USART2;					    //USART1
	UART2_Handler.Init.BaudRate=bound;				    //波特率
	UART2_Handler.Init.WordLength=UART_WORDLENGTH_8B;   //字長為8位資料格式
	UART2_Handler.Init.StopBits=UART_STOPBITS_1;	    //一個停止位
	UART2_Handler.Init.Parity=UART_PARITY_NONE;		    //無奇偶校驗位
	UART2_Handler.Init.HwFlowCtl=UART_HWCONTROL_NONE;   //無硬體流控
	UART2_Handler.Init.Mode=UART_MODE_TX_RX;		    //收發模式
	HAL_UART_Init(&UART2_Handler);					    //HAL_UART_Init()會使能UART2
	
	HAL_UART_Receive_IT(&UART2_Handler, (u8 *)aRxBuffer2, RXBUFFERSIZE);//該函式會開啟接收中斷：標志位UART_IT_RXNE，并且設定接識訓沖以及接識訓沖接收最大資料量
}

//串口3決議SBUS信號，100k波特率，2個停止位，偶校驗
void uart3_init(u32 bound)
{
	//UART3 初始化設定
	UART3_Handler.Instance=USART3;					    //USART1
	UART3_Handler.Init.BaudRate=bound;				    //波特率
	UART3_Handler.Init.WordLength=UART_WORDLENGTH_8B;   //字長為8位資料格式
	UART3_Handler.Init.StopBits=UART_STOPBITS_1;	    //2個停止位
	UART3_Handler.Init.Parity=UART_PARITY_NONE;		    //偶校驗位
	UART3_Handler.Init.HwFlowCtl=UART_HWCONTROL_NONE;   //無硬體流控
	UART3_Handler.Init.Mode=UART_MODE_TX_RX;		    //收發模式
	HAL_UART_Init(&UART3_Handler);					    //HAL_UART_Init()會使能UART1
	
	HAL_UART_Receive_IT(&UART3_Handler, (u8 *)aRxBuffer3, RXBUFFERSIZE);//該函式會開啟接收中斷：標志位UART_IT_RXNE，并且設定接識訓沖以及接識訓沖接收最大資料量
}

void uart4_init(u32 bound)
{
	//UART 初始化設定
	UART4_Handler.Instance = UART4;					//USART1
	UART4_Handler.Init.BaudRate = bound;				//波特率
	UART4_Handler.Init.WordLength = UART_WORDLENGTH_9B; //字長為8位資料格式
	UART4_Handler.Init.StopBits = UART_STOPBITS_1;		//一個停止位
	UART4_Handler.Init.Parity = UART_PARITY_EVEN;		//無奇偶校驗位
	UART4_Handler.Init.HwFlowCtl = UART_HWCONTROL_NONE; //無硬體流控
	UART4_Handler.Init.Mode = UART_MODE_TX_RX;			//收發模式
	HAL_UART_Init(&UART4_Handler);						//HAL_UART_Init()會使能UART1

	HAL_UART_Receive_IT(&UART4_Handler, (u8 *)aRxBuffer4, RXBUFFERSIZE); //該函式會開啟接收中斷：標志位UART_IT_RXNE，并且設定接識訓沖以及接識訓沖接收最大資料量
}
//UART底層初始化，時鐘使能，引腳配置，中斷配置
//此函式會被HAL_UART_Init()呼叫
//huart:串口句柄

void HAL_UART_MspInit(UART_HandleTypeDef *huart)
{
	//GPIO埠設定
	GPIO_InitTypeDef GPIO_Initure;
	if (huart->Instance == USART1) //如果是串口1，進行串口1 MSP初始化
	{
		__HAL_RCC_GPIOA_CLK_ENABLE();  //使能GPIOA時鐘
		__HAL_RCC_USART1_CLK_ENABLE(); //使能USART1時鐘

		GPIO_Initure.Pin = GPIO_PIN_9;			  //PA9
		GPIO_Initure.Mode = GPIO_MODE_AF_PP;	  //復用推挽輸出
		GPIO_Initure.Pull = GPIO_PULLUP;		  //上拉
		GPIO_Initure.Speed = GPIO_SPEED_FAST;	  //高速
		GPIO_Initure.Alternate = GPIO_AF7_USART1; //復用為USART1
		HAL_GPIO_Init(GPIOA, &GPIO_Initure);	  //初始化PA9

		GPIO_Initure.Pin = GPIO_PIN_10;		 //PA10
		HAL_GPIO_Init(GPIOA, &GPIO_Initure); //初始化PA10

#if EN_USART1_RX
		HAL_NVIC_EnableIRQ(USART1_IRQn);		 //使能USART1中斷通道
		HAL_NVIC_SetPriority(USART1_IRQn, 3, 2); //搶占優先級3，子優先級3
#endif
	}

	if(huart->Instance==USART2)//如果是串口3，進行串口3 MSP初始化
	{
		__HAL_RCC_GPIOA_CLK_ENABLE();			//使能GPIOB時鐘
		__HAL_RCC_USART2_CLK_ENABLE();			//使能USART1時鐘
	
		GPIO_Initure.Pin=GPIO_PIN_2;			//PA2 TX
		GPIO_Initure.Mode=GPIO_MODE_AF_PP;		//復用推挽輸出
		GPIO_Initure.Pull=GPIO_PULLUP;			//上拉
		GPIO_Initure.Speed=GPIO_SPEED_FAST;		//高速
		GPIO_Initure.Alternate=GPIO_AF7_USART2;	//復用為USART3
		HAL_GPIO_Init(GPIOA,&GPIO_Initure);	   	//初始化PA9

		GPIO_Initure.Pin=GPIO_PIN_3;			//PA3 RX
		HAL_GPIO_Init(GPIOA,&GPIO_Initure);	   	//初始化PA10
		
#if EN_USART2_RX
		HAL_NVIC_EnableIRQ(USART2_IRQn);				//使能USART1中斷通道
		HAL_NVIC_SetPriority(USART2_IRQn,3,3);			//搶占優先級3，子優先級3
#endif	
	}

	if(huart->Instance==USART3)//如果是串口3，進行串口3 MSP初始化
	{
		__HAL_RCC_GPIOB_CLK_ENABLE();			//使能GPIOB時鐘
		__HAL_RCC_USART3_CLK_ENABLE();			//使能USART1時鐘
	
		GPIO_Initure.Pin=GPIO_PIN_10;			//PB10 TX
		GPIO_Initure.Mode=GPIO_MODE_AF_PP;		//復用推挽輸出
		GPIO_Initure.Pull=GPIO_PULLUP;			//上拉
		GPIO_Initure.Speed=GPIO_SPEED_FAST;		//高速
		GPIO_Initure.Alternate=GPIO_AF7_USART3;	//復用為USART3
		HAL_GPIO_Init(GPIOB,&GPIO_Initure);	   	//初始化PA9

		GPIO_Initure.Pin=GPIO_PIN_11;			//PB11 RX
		HAL_GPIO_Init(GPIOB,&GPIO_Initure);	   	//初始化PA10
		
#if EN_USART3_RX
		HAL_NVIC_EnableIRQ(USART3_IRQn);				//使能USART1中斷通道
		HAL_NVIC_SetPriority(USART3_IRQn,2,1);			//搶占優先級3，子優先級3
#endif	
	}

	if (huart->Instance == UART4) //如果是串口1，進行串口1 MSP初始化
	{
		__HAL_RCC_GPIOD_CLK_ENABLE();  //使能GPIOA時鐘
		__HAL_RCC_UART4_CLK_ENABLE(); //使能USART1時鐘

		GPIO_Initure.Pin = GPIO_PIN_0;			  //PA9
		GPIO_Initure.Mode = GPIO_MODE_AF_PP;	  //復用推挽輸出
		GPIO_Initure.Pull = GPIO_PULLUP;		  //上拉
		GPIO_Initure.Speed = GPIO_SPEED_FAST;	  //高速
		GPIO_Initure.Alternate = GPIO_AF8_UART4; //復用為USART1
		HAL_GPIO_Init(GPIOD, &GPIO_Initure);	  //初始化PA9

		GPIO_Initure.Pin = GPIO_PIN_1;		 //PA10
		HAL_GPIO_Init(GPIOD, &GPIO_Initure); //初始化PA10

#if EN_USART4_RX
		HAL_NVIC_EnableIRQ(UART4_IRQn);		 //使能USART1中斷通道
		HAL_NVIC_SetPriority(UART4_IRQn, 3, 2); //搶占優先級3，子優先級3
#endif
	}
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	int i;
	while (huart->Instance == USART1) //如果是串口1
	{
		USART1_RX_BUF[USART1_RX_STA] = aRxBuffer1[0];
		if (USART1_RX_STA == 0 && USART1_RX_BUF[USART1_RX_STA] != 0x0F) break; //幀頭不對，丟掉
		USART1_RX_STA++;
		if (USART1_RX_STA > USART_REC_LEN) USART1_RX_STA = 0;  ///接收資料錯誤,重新開始接收

//		if (USART1_RX_BUF[0] == 0x0F && USART1_RX_BUF[24] == 0x00 && USART1_RX_STA == 25)	//接受完一幀資料
		if (USART1_RX_BUF[0] == 0x0F && USART1_RX_STA == 25)	//接受完一幀資料
		{
			update_sbus(USART1_RX_BUF);
			for (i = 0; i<25; i++)
			{
				USART1_RX_BUF[i] = 0;
			}
			USART1_RX_STA = 0;
		#ifdef ENABLE_IWDG
			IWDG_Feed();    			//喂狗		//超過時間沒有收到遙控器的資料會復位
		#endif
		}
		break;
	}

	if(huart->Instance==USART2)//如果是串口2
	{
		CopeSerial2Data(aRxBuffer2[0]);//處理數
	}

	while(huart->Instance==USART3)//如果是串口3
	{
		
		break;
	}

	while (huart->Instance == UART4) //如果是串口1
	{
		
		break;
	}
}


//CopeSerialData為串口2中斷呼叫函式，串口每收到一個資料，呼叫一次這個函式，
void CopeSerial2Data(unsigned char ucData)
{
	static unsigned char ucRxBuffer[250];
	static unsigned char ucRxCnt = 0;	
	
	ucRxBuffer[ucRxCnt++]=ucData;
	if (ucRxBuffer[0]!=0x55) //資料頭不對，則重新開始尋找0x55資料頭
	{
		ucRxCnt=0;
		return;
	}
	if (ucRxCnt<11) {return;}//資料不滿11個，則回傳
	else
	{
		switch(ucRxBuffer[1])
		{
			case 0x50:	memcpy(&stcTime,&ucRxBuffer[2],8);break;//memcpy為編譯器自帶的記憶體拷貝函式，需參考"string.h"，將接識訓沖區的字符拷貝到資料共同體里面，從而實作資料的決議，
			case 0x51:	memcpy(&stcAcc,&ucRxBuffer[2],8);break;
			case 0x52:	memcpy(&stcGyro,&ucRxBuffer[2],8);break;
			case 0x53:	memcpy(&stcAngle,&ucRxBuffer[2],8);break;
			case 0x54:	memcpy(&stcMag,&ucRxBuffer[2],8);break;
			case 0x55:	memcpy(&stcDStatus,&ucRxBuffer[2],8);break;
			case 0x56:	memcpy(&stcPress,&ucRxBuffer[2],8);break;
			case 0x57:	memcpy(&stcLonLat,&ucRxBuffer[2],8);break;
			case 0x58:	memcpy(&stcGPSV,&ucRxBuffer[2],8);break;
			case 0x59:  memcpy(&stcQuat,&ucRxBuffer[2],8);break;
		}
		ucRxCnt=0;
	}
}

//串口1中斷服務程式
void USART1_IRQHandler(void)
{
	u32 timeout = 0;
	u32 maxDelay = 0x1FFFF;
#if SYSTEM_SUPPORT_OS //使用OS
	OSIntEnter();
#endif

	HAL_UART_IRQHandler(&UART1_Handler); //呼叫HAL庫中斷處理公用函式

	timeout = 0;
	while (HAL_UART_GetState(&UART1_Handler) != HAL_UART_STATE_READY) //等待就緒
	{
		timeout++; 超時處理
		if (timeout > maxDelay)
			break;
	}

	timeout = 0;
	while (HAL_UART_Receive_IT(&UART1_Handler, (u8 *)aRxBuffer1, RXBUFFERSIZE) != HAL_OK) //一次處理完成之后，重新開啟中斷并設定RxXferCount為1
	{
		timeout++; //超時處理
		if (timeout > maxDelay)
			break;
	}
#if SYSTEM_SUPPORT_OS //使用OS
	OSIntExit();
#endif
}

//串口2中斷服務程式
void USART2_IRQHandler(void)                	
{ 
	u32 timeout=0;
    u32 maxDelay=0x1FFFF;
#if SYSTEM_SUPPORT_OS	 	//使用OS
	OSIntEnter();    
#endif
	
	HAL_UART_IRQHandler(&UART2_Handler);	//呼叫HAL庫中斷處理公用函式
	
	timeout=0;
    while (HAL_UART_GetState(&UART2_Handler)!=HAL_UART_STATE_READY)//等待就緒
	{
        timeout++;超時處理
        if(timeout>maxDelay) break;		
	}
     
	timeout=0;
	while(HAL_UART_Receive_IT(&UART2_Handler,(u8 *)aRxBuffer2, RXBUFFERSIZE)!=HAL_OK)//一次處理完成之后，重新開啟中斷并設定RxXferCount為1
	{
        timeout++; //超時處理
        if(timeout>maxDelay) break;	
	}
#if SYSTEM_SUPPORT_OS	 	//使用OS
	OSIntExit();  											 
#endif
} 

//串口3中斷服務程式
void USART3_IRQHandler(void)                	
{ 
	u32 timeout=0;
    u32 maxDelay=0x1FFFF;
#if SYSTEM_SUPPORT_OS	 	//使用OS
	OSIntEnter();    
#endif
	
	HAL_UART_IRQHandler(&UART3_Handler);	//呼叫HAL庫中斷處理公用函式
	
	timeout=0;
    while (HAL_UART_GetState(&UART3_Handler)!=HAL_UART_STATE_READY)//等待就緒
	{
        timeout++;超時處理
        if(timeout>maxDelay) break;		
	}
     
	timeout=0;
	while(HAL_UART_Receive_IT(&UART3_Handler,(u8 *)aRxBuffer3, RXBUFFERSIZE)!=HAL_OK)//一次處理完成之后，重新開啟中斷并設定RxXferCount為1
	{
        timeout++; //超時處理
        if(timeout>maxDelay) break;	
	}
#if SYSTEM_SUPPORT_OS	 	//使用OS
	OSIntExit();  											 
#endif
} 


//串口4中斷服務程式
void UART4_IRQHandler(void)
{
	u32 timeout = 0;
	u32 maxDelay = 0x1FFFF;
#if SYSTEM_SUPPORT_OS //使用OS
	OSIntEnter();
#endif

	HAL_UART_IRQHandler(&UART4_Handler); //呼叫HAL庫中斷處理公用函式

	timeout = 0;
	while (HAL_UART_GetState(&UART4_Handler) != HAL_UART_STATE_READY) //等待就緒
	{
		timeout++; 超時處理
		if (timeout > maxDelay)
			break;
	}

	timeout = 0;
	while (HAL_UART_Receive_IT(&UART4_Handler, (u8 *)aRxBuffer4, RXBUFFERSIZE) != HAL_OK) //一次處理完成之后，重新開啟中斷并設定RxXferCount為1
	{
		timeout++; //超時處理
		if (timeout > maxDelay)
			break;
	}
#if SYSTEM_SUPPORT_OS //使用OS
	OSIntExit();
#endif
}

串口的驅動代碼很簡單，我們主要看中斷服務程式怎么處理的，定位到上面的void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)，這里面包含了所有串口的中斷服務程式，串口1前面講了是接收遙控器接識訓SBUS信號，串口2的中斷里面只有一行，就是呼叫了CopeSerial2Data(aRxBuffer2[0])，定義也在上邊，單獨貼出來：

void CopeSerial2Data(unsigned char ucData)
{
	static unsigned char ucRxBuffer[250];
	static unsigned char ucRxCnt = 0;	
	
	ucRxBuffer[ucRxCnt++]=ucData;
	if (ucRxBuffer[0]!=0x55) //資料頭不對，則重新開始尋找0x55資料頭
	{
		ucRxCnt=0;
		return;
	}
	if (ucRxCnt<11) {return;}//資料不滿11個，則回傳
	else
	{
		switch(ucRxBuffer[1])
		{
			case 0x50:	memcpy(&stcTime,&ucRxBuffer[2],8);break;//memcpy為編譯器自帶的記憶體拷貝函式，需參考"string.h"，將接識訓沖區的字符拷貝到資料共同體里面，從而實作資料的決議，
			case 0x51:	memcpy(&stcAcc,&ucRxBuffer[2],8);break;
			case 0x52:	memcpy(&stcGyro,&ucRxBuffer[2],8);break;
			case 0x53:	memcpy(&stcAngle,&ucRxBuffer[2],8);break;
			case 0x54:	memcpy(&stcMag,&ucRxBuffer[2],8);break;
			case 0x55:	memcpy(&stcDStatus,&ucRxBuffer[2],8);break;
			case 0x56:	memcpy(&stcPress,&ucRxBuffer[2],8);break;
			case 0x57:	memcpy(&stcLonLat,&ucRxBuffer[2],8);break;
			case 0x58:	memcpy(&stcGPSV,&ucRxBuffer[2],8);break;
			case 0x59:  memcpy(&stcQuat,&ucRxBuffer[2],8);break;
		}
		ucRxCnt=0;
	}
}

其實就是按照我們上面講到的串口通信協議，一次讀完一幀資料（11個位元組）后，根據第二個位元組的ID判斷是什么資料幀，然后存入到相應的記憶體區，這些記憶體區前面HT905.c中已經定義過了，

然后回到剛才創建的sensor.c，剛才在里面加了void IIC_ReadJY901(float *Gyro, float *Acc, float *Mag, float *Angle)，實作了IIC讀取慣導資料，繼續添加函式void UART_ReadIMU(float *Gyro, float *Acc, float *Mag, float *Angle)，實作同樣的讀取資料功能，只不過這個使用串口資料讀的，

//串口讀HWT905
void UART_ReadIMU(float *Gyro, float *Acc, float *Mag, float *Angle)
{
	 //加速度值
    Acc[0] = (float)stcAcc.a[0]/32768*16;
    Acc[1] = (float)stcAcc.a[1]/32768*16;
    Acc[2] = (float)stcAcc.a[2]/32768*16;
    //角速度值
    Gyro[0] = (float)stcGyro.w[0]/32768*2000;
    Gyro[1] = (float)stcGyro.w[1]/32768*2000;
    Gyro[2] = (float)stcGyro.w[2]/32768*2000;
    //磁力計值
    Mag[0] = stcMag.h[0];
    Mag[1] = stcMag.h[1];
    Mag[2] = stcMag.h[2];
    //姿態角
    Angle[0] = (float)stcAngle.Angle[0]/32768*180;
    Angle[1] = (float)stcAngle.Angle[1]/32768*180;
    Angle[2] = (float)stcAngle.Angle[2]/32768*180;
}

這個讀出來的資料還是要濾一下波的，請往下看，

四. 傳感任務應用程式

1. 姿態角滑動平均濾波

通過上面一節的代碼已經實作了IIC和UART兩種方式讀取傳感器的資料值了，下面繼續寫應用程式，在到達我們的主函式中的傳感任務之前，還要解決前面說到的濾波問題，雖然傳感器內部是進行了卡爾曼濾波的，但是不妨礙我們繼續用一個環形濾波器再濾一次，也叫滑動平均濾波哈，專業的名字應該叫有限沖擊回應濾波（FIR），原理很簡單，就是取最近的N次資料取平均，

在sensor.c中添加以下全域變數，我們需要的是三軸角度、角速度，一共六個量，各建立一個陣列，存放最近十次的資料，sum開頭的變數是后面求和取平均用，

// 慣導值濾波引數
float filterAngleYaw[10];  //濾波
float filterAngleRoll[10];
float filterAnglePitch[10];
float sumYaw,sumRoll,sumPitch;

float filterAngleYawRate[10];  //濾波
float filterAngleRollRate[10];
float filterAnglePitchRate[10];
float sumYawRate,sumRollRate,sumPitchRate;

再添加函式，void sensorReadAngle(float *Gyro, float *Angle)，它干的事情其實就是取最近的十次資料進行平均，

// FIR濾波
void sensorReadAngle(float *Gyro, float *Angle)
{
	float gyro[3], acc[3],mag[3],angle[3];
	float gyro1[3], angle1[3];
	float gyro2[3], angle2[3];
	float tempYaw,tempRoll,tempPitch;
	float tempYawRate,tempRollRate,tempPitchRate;
	u8 i;

	if (command[IMU_MODE] == JY901) 
		IIC_ReadJY901(gyro, acc, mag, angle);
	if (command[IMU_MODE] == HT905)
		UART_ReadIMU(gyro, acc, mag, angle);
	if (command[IMU_MODE] == JY901_HT905)	
	{
		IIC_ReadJY901(gyro1, acc, mag, angle1);
		UART_ReadIMU(gyro2, acc, mag, angle2);
		for (i=0; i<3;i++)
		{
			gyro[i] = (gyro1[i] + gyro2[i])/2;
			angle[i] = (angle1[i] + angle2[i])/2;
		}
	}
	tempRoll = filterAngleRoll[count];
	tempPitch = filterAnglePitch[count];
	tempYaw = filterAngleYaw[count];
	filterAngleRoll[count] = angle[0];
	filterAnglePitch[count] = angle[1];
	filterAngleYaw[count] = angle[2];
	sumRoll += filterAngleRoll[count] - tempRoll;
	sumPitch += filterAnglePitch[count] - tempPitch;
	sumYaw += filterAngleYaw[count] - tempYaw;
	Angle[0] = sumRoll/10.0f;
	Angle[1] = sumPitch/10.0f;
	Angle[2] = sumYaw/10.0f;
	
	tempRollRate = filterAngleRollRate[count];
	tempPitchRate = filterAnglePitchRate[count];
	tempYawRate = filterAngleYawRate[count];
	filterAngleRollRate[count] = gyro[0];
	filterAnglePitchRate[count] = gyro[1];
	filterAngleYawRate[count] = gyro[2];
	sumRollRate += filterAngleRollRate[count] - tempRollRate;
	sumPitchRate += filterAnglePitchRate[count] - tempPitchRate;
	sumYawRate += filterAngleYawRate[count] - tempYawRate;
	Gyro[0] = sumRollRate/10.0f;
	Gyro[1] = sumPitchRate/10.0f;
	Gyro[2] = sumYawRate/10.0f;
	
	count++;
	if (count == 10) count = 0;
}

然后這里面我設定了一個三段開關用來選擇傳感器的資料來源，分別是單獨使用JY901、單獨使用GPSIMU和使用兩個資料的平均，

終于封裝完了，下面進入我們的終極目標，main檔案，補充傳感任務，

2. 傳感任務創建

下面進入我們的終極目標main.c，因為之前我們所做的所有作業都是做驅動和封裝，現在我們想要讀角度和角資料只需要在main.c中呼叫void sensorReadAngle(float *Gyro, float *Angle)就行了，在上一節STM32實作水下四旋翼（三）通信任務——遙控器SBUS通信中我們創建和實作了遙控器通信任務（communicate_task），現在我們在main.c中創建一個新的任務——傳感任務（sensor_task），接上一節main.c的基礎上添加：

//任務優先級
#define START_TASK_PRIO 3
//任務堆疊大小
#define START_STK_SIZE 128
//任務控制塊
OS_TCB StartTaskTCB;
//任務堆疊
CPU_STK START_TASK_STK[START_STK_SIZE];
//任務函式
void start_task(void *p_arg);

//communicate任務
//設定任務優先級
#define COMMUNICATE_TASK_PRIO 5					// SBUS 信號的更新是在串口中斷中進行的
//任務堆疊大小
#define COMMUNICATE_STK_SIZE 512
//任務控制塊
OS_TCB CommunicateTaskTCB;
//任務堆疊
CPU_STK COMMUNICATE_TASK_STK[COMMUNICATE_STK_SIZE];
//led0任務
void communicate_task(void *p_arg);

//sensorTask 引數配置任務 在線除錯引數并寫入flash
//設定任務優先級
#define SENSOR_TASK_PRIO 6
//任務堆疊大小
#define SENSOR_STK_SIZE 512
//任務控制塊
OS_TCB SensorTaskTCB;
//任務堆疊
CPU_STK SENSOR_TASK_STK[SENSOR_STK_SIZE];
//motor任務
u8 sensor_task(void *p_arg);

在void start_task(void *p_arg)中添加任務創建函式：

//Sensor任務
OSTaskCreate((OS_TCB *)&SensorTaskTCB,
			 (CPU_CHAR *)"Sensor task",
			 (OS_TASK_PTR)sensor_task,
			 (void *)0,
			 (OS_PRIO)SENSOR_TASK_PRIO,
			 (CPU_STK *)&SENSOR_TASK_STK[0],
			 (CPU_STK_SIZE)SENSOR_STK_SIZE / 10,
			 (CPU_STK_SIZE)SENSOR_STK_SIZE,
			 (OS_MSG_QTY)0,
			 (OS_TICK)10,
			 (void *)0,
			 (OS_OPT)OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR |OS_OPT_TASK_SAVE_FP,
			 (OS_ERR *)&err);

3. 傳感任務中應用程式

任務創建完了，再增加任務函式：

u8 sensor_task(void *p_arg)
{
	OS_ERR err;
	CPU_SR_ALLOC();

	float Gyro[3], Angle[3];
	u8 count = 20;

	//濾波初始化
	while (count--)
	{
		sensorReadAngle(Gyro, Angle);
	}

	// 初始化之后，所有期望值復制為實際值
	state.realAngle.roll = Angle[0];
	state.realAngle.pitch = Angle[1];
	state.realAngle.yaw = Angle[2];
	state.realDepth = wDepth;
	setstate.expectedAngle.roll = state.realAngle.roll;
	setstate.expectedAngle.pitch = state.realAngle.pitch;
	setstate.expectedAngle.yaw = state.realAngle.yaw; //初始化之后將當前的姿態角作為期望姿態角初值

	while (1)
	{
		/********************************************** 獲取期望值與測量值*******************************************/
		sensorReadAngle(Gyro, Angle);
		//反饋值
		state.realAngle.roll = Angle[0];
		state.realAngle.pitch = Angle[1];
		state.realAngle.yaw = Angle[2];
		state.realRate.roll = Gyro[0];
		state.realRate.pitch = Gyro[1];
		state.realRate.yaw = Gyro[2];
		
		delay_ms(5); // 水深傳感器單次讀取需要20ms+, 所以這里的延時小一點
	}
}

看看傳感任務里面干了啥，定義了兩個陣列Gyro[3], Angle[3]，存放三軸角度和角速度，開始先進行濾波初始化（記得之前的環形濾波器么，剛開始的資料是不對的，所以這里設定先讀20次等資料穩定），然后讀一次資料，對state和setstate進行初始化，這兩個結構體變數前面講過，分別是機器人的實際狀態和設定狀態，如果不記得請跳轉到 STM32實作水下四旋翼（五）自定義航行資料中查看，

之后進入while(1)回圈，按照5ms的采樣周期讀取角度、角速度資料，呼叫sensorReadAngle(Gyro, Angle)即可，然后更新state狀態值，傳感任務中的讀姿態角就到此結束啦，圓滿完成任務，后面還會在傳感任務中增加讀深度、讀電壓等內容，

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/265994.html

標籤：其他

上一篇：嵌入式面試—專案篇（二）全國大學生智能汽車競賽（團隊專案）

下一篇：Eclipse Paho MQTT Python Client 使用手冊

STM32實作水下四旋翼（六）傳感任務2——姿態解算代碼實作（使用角度傳感器）

目錄