STM32实现水下四旋翼（六）传感任务2——姿态解算代码实现（使用角度传感器）

一. 绪论

上一篇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

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

我们只关注几个重要的数据：

加速度输出：

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

分别为三轴的加速度和温度高低字节。

角速度输出：

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

分别为三轴的角速度和温度高低字节。

角度输出：

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

分别为三轴的角度和温度高低字节。

磁场输出：

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

分别为三轴的磁场强度和温度高低字节。

其他还有气压高度输出、经纬度输出、四元数输出、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];
	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状态值，传感任务中的读姿态角就到此结束啦，圆满完成任务。后面还会在传感任务中增加读深度、读电压等内容。