上一篇STM32实现水下四旋翼（四）传感任务1——姿态解算原理篇（这篇还在写，暂时不要点~）中我们一起复习了传感器测量原理与状态估计的理论知识，这些内容非常非常重要，但很遗憾的是在本系列文章中没有用到。。因为我使用的是角度传感器，通过串口直接读取三轴角度。至于传感器内部，它就是使用的卡尔曼滤波进行角度估计的，所以我们省略了一步骤。如果是使用MPU6050、MPU9050这类传感器，那么就需要使用互补滤波或卡尔曼滤波算法进行姿态解算了。
长文预警！！！因为代码确实太多了。请耐心看下去，想学东西的一定不会失望哈！
水下四旋翼使用了两个角度传感器——维特智能的JY901B和十轴惯导JY-GPSIMU，价格分别为100多和500多，如图所示（声明，不是打广告，你上淘宝搜角度传感器基本都是他们家的，使用确实很方便）。这两个器件都能测气压、高度，十轴惯导还带GPS 。使用两个传感器是冗余设计，有的要求高的场合用三个四个的都有。至于怎么去使用多个，是只用一个还是多个数据取平均都可以灵活调整。
JY901B的数据输出是IIC和UART两种方式，十轴惯导只有UART输出，本文分别用IIC和UART2读两个器件的角度数据。另外JY901B最好焊在板子上，或者用引脚对插，总之注意惯性器件安装一定要稳固。
（使用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（查表得），连续读取六个字节即可。
（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读取还是串口上传，都是原始数据，需要经过公式换算得到实际值，换算公式传感器说明书有给，后面的程序里面也会看到。
创建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)读取一次三轴角度、角速度、加速度值了。不过这还不够，还需要滤下波，请往下看。
串口读取数据就很简单了，按照通信协议去解析就行了，这里参考厂家官方例程。创建一个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;
}
这个读出来的数据还是要滤一下波的，请往下看。
通过上面一节的代码已经实现了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文件，补充传感任务。
下面进入我们的终极目标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);
任务创建完了，再增加任务函数：
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状态值，传感任务中的读姿态角就到此结束啦，圆满完成任务。后面还会在传感任务中增加读深度、读电压等内容。