STM32实现四驱小车（三）传感任务——姿态角解算

一. 绪论

本文接上一篇STM32实现四驱小车（二）通信任务——遥控器SBUS通信，在今天的文章中解决小车的姿态角测量问题，实现操作系统中的传感任务。对小车来说只需要测量航向角（YAW），方法就是使用姿态角传感器，通过串口直接读三轴角度，简单的不能再简单有木有。但是为了知识体系的完整性（也为了增加文章的逼格哈哈），本文会从传感器测量原理到姿态估计讲起，讲解标准对标多旋翼的姿态解算与状态估计。本文的结构是高高拿起，轻轻放下。众位看官如果只是想实现功能看最后二节代码部分就行了。

本文理论部分主要参考了全权老师的《多旋翼飞行器设计与控制》。

二. 惯性传感器测量原理

1. 三轴加速度计

三轴加速度计是一种惯性传感器，能够测量物体的比力，即去掉重力后的整体加速度。这里我们需要记住，使用三轴加速度计的最重要的功能是测量角度，如图所示，弹簧压缩量由加速度计与地面的角度决定，比力能通过弹簧压缩长度来测量。因此在没有外力作用的情况下，加速度计能够准确的测量俯仰角和滚转角，且没有累计误差。 这句话很重要，它是实现互补滤波、卡尔曼滤波的基础。大家只需要知道加速度计的特性是低频特性好，高频特性差。

这里直接给出加速度计近似测量俯仰角和滚转角的公式：
$${\theta }_m=\arcsin \left(\frac{a_{x_{b}m}}{g}\right),{\varphi }_m=-\arcsin \left(\frac{a_{y_{b}m}}{g\cos {\theta }_m}\right)$$

2. 三轴陀螺仪

MEMS陀螺仪是基于科里奥利力工作的传感器，科里奥里力是对直线运动物体相对于旋转坐标系产生偏移的一种直观描述。因为科里奥利力正比于角速度，所以根据电容量的变化可以计算角速度。总之，大家知道这玩意可以测量三轴角速度就行了，三轴角速度积分就可以求得三轴角度。它的特点是高频特性好，低频特性差。

3. 三轴磁力计

磁力计是提供导航及基于位置服务的重要组成部分，一般利用各向异性磁致电阻或者霍尔效应来检测空间中的磁感应强度。此外，基于洛伦兹力的磁力计在不断研究和发展。基于洛伦兹力原理，电磁场会激发电磁力，进而改变电路中的电容大小……

大家只需要知道这玩意是测量三轴磁场强度的就行了，其中最重要的是测量偏航角。磁力计测量的偏航角在滤波中将会作为观测值。

三. 状态估计

状态估计是控制与决策的基础，具有十分重要的作用。传感器的测量存在大量噪声，一方面，有些信息无法直接测量得到，需要被估计出来，另一方面，传感器的信息存在冗余。所以需要进行多传感器融合来提高状态估计的精确性和鲁棒性。对机器人来说，状态估计包括姿态估计、速度估计、位置估计等。在本文中我们只关注姿态估计。

1. 姿态估计

姿态估计的主要目的是估计姿态角，姿态角的表示方法有欧拉角、旋转矩阵、四元数三种常见形式。姿态信息主要利用三轴加速度计、三轴陀螺仪、三轴磁力计的数据，通过互补滤波或卡尔曼滤波获得。三轴陀螺仪的动态响应快，测量精度高，但求解姿态角时需要对角速度积分，会产生累计误差（积分漂移）。 三轴加速度计和三轴磁力计可以得到稳定的姿态角，无累计误差，但动态响应特性差、测量噪声大。

（1）线性互补滤波器

这里先下一些定义，三轴姿态角向量表示为$s={[\varphi ,\theta ,\psi ]}^T$，分别表示滚转角、俯仰角、偏航角。机体旋转角速度为${}^{\text{b}}w={[w_{x_b},w_{y_b},w_{z_b}]}^T$，在滚转角和俯仰角变化较小的情况下可以近似为 ${[\dot{\varphi },\dot{\theta },\dot{\psi }]}^T={[w_{x_b},w_{y_b},w_{z_b}]}^T$

①俯仰角

俯仰角的拉氏变换表示为
$$\theta (s)=\frac{1}{\tau s+1}\theta (s)+\frac{\tau s}{\tau s+1}\theta (s)$$
其中$1/(\tau s+1)$ 表示低通滤波器的传递函数，$\tau \in {\mathbb{R}}_{\text{+}}$表示时间常数； $\tau s/(\tau s+1)$表示高通滤波器的传递函数。考虑到加速度计测量得到的俯仰角无漂移，但噪声大，可将俯仰角测量值建模为(加速度计的测量值)：
$${\theta }_m=\theta +n_{\theta }$$
其中，$n_{\theta }\in \mathbb{R}$ 表示高频噪声，$\theta$ 表示俯仰角真值。

考虑到角速度积分得到的俯仰角噪声小，但漂移大，可以建模为
$$\frac{w_{y_{b}m}(s)}{s}=\theta (s)+c\frac{1}{s}$$
其中，$w_{y_{b}m}(s)/s$ 表示对角速率积分得到俯仰角的拉氏变换，$c/s$ 表示常值漂移的拉氏变换， $w_{y_{b}m}$是陀螺仪测量值。

因此针对俯仰角，线性互补滤波器的标准形式表示为
$$\hat{\theta }(s)=\frac{1}{\tau s+1}{\theta }_m(s)+\frac{\tau s}{\tau s+1}\left(\frac{1}{s}{w}_{y_{b}m}(s)\right)$$
下面将给出线性互补滤波器能够精确估计姿态角的解释。将上式整理为
$$\hat{\theta }(s)=\theta (s)+\left(\frac{1}{\tau s+1}{n}_{\theta }(s)+\frac{\tau s}{\tau s+1}c\frac{1}{s}\right)$$
可以看出，高频噪声${\theta }_m(s)$ 通过低通滤波器 $1/(\tau s+1)$基本上衰减为0，而低频信号 $c/s$通过高通滤波器$\tau s/(\tau s+1)$ 也基本衰减为0。因此可以认为 $\hat{\theta }(s)\approx \theta (s)$。在整个过程中，低通滤波器将加速度计测量值无漂移的优势保留下来，而高通滤波器将陀螺仪噪声小的优势保留下来，互补滤波器实现了无偏估计。

为了用计算机（单片机）实现滤波器算法，需要将滤波器转换为离散时间差分形式。通过一阶向后差分法，将$s$表示为$s=(1-z^{-1})/T_s$，上面的互补滤波器标准形式转化为
$$\hat{\varphi }(k)=\frac{\tau }{\tau +T_s}\left(\hat{\varphi }(k-1)+T_s{w}_{x_{b}m}(k)\right)+\frac{T_s}{\tau +T_s}{\varphi }_m(k)$$
取$\tau /(\tau +T_s)=0.95$ ，则$T_s/(\tau +T_s)=0.05$ ，控制器实现俯仰角解算的滤波算法为：
$$\hat{\theta }(k)=0.95\left(\hat{\theta }(k-1)+T_s{w}_{y_{b}m}(k)\right)+0.05{\theta }_m(k)$$

到这里单片机实现互补滤波的算法就出来了，Ts是采样周期，$w_{y_{b}m}(k)$与${\theta }_m(k)$分别是第k次测量得到的陀螺仪俯仰角速度测量值和加速度计俯仰角测量值。

② 滚转角

滚转角的原理和俯仰角的原理是一样的，这里就不推导了，直接给出结果。
$$\hat{\varphi }(k)=\frac{\tau }{\tau +T_s}\left(\hat{\varphi }(k-1)+T_s{w}_{x_{b}m}(k)\right)+\frac{T_s}{\tau +T_s}{\varphi }_m(k)$$
取$\tau /(\tau +T_s)=0.95$ ，则$T_s/(\tau +T_s)=0.05$ ，控制器实现滚转角解算的滤波算法为：
$$\hat{\varphi }(k)=0.95\left(\hat{\varphi }(k-1)+T_s{w}_{x_{b}m}(k)\right)+0.05{\varphi }_m(k)$$

③ 偏航角

航向角的互补滤波与滚转角、俯仰角不一样。俯仰角和滚转角的观测值来自于加速度计，而加速度计无法测量航向角，所以需要另外寻找航向角的观测值。三轴磁力计可以提供准确的偏航角，此外GPS接收机也可以提供偏航角。将两者测量得到的偏航角分别表示为${\psi }_{\text{GPS}}$和${\psi }_{\text{mag}}$。一种方法是定义偏航角为两者的加权和。即
$${\psi }_{\mathrm{m}}=\left(1-{\alpha }_{\psi }\right){\psi }_{\mathrm{G}\mathrm{P}\mathrm{S}}+{\alpha }_{\psi }{\psi }_{\mathrm{m}\mathrm{a}\mathrm{g}}$$
${\alpha }_{\psi }\in [0,1]$是加权因子，它决定了是更相信GPS还是更相信磁罗盘。

得到${\psi }_{\text{m}}$之后，得到偏航角估计为
$$\hat{\psi }(k)=\frac{\tau }{\tau +T_s}\left(\hat{\psi }(k-1)+T_s{w}_{z_{b}m}(k)\right)+\frac{T}{\tau +T_s}{\psi }_m(k)$$
取$\tau /(\tau +T_s)=0.95$ ，则$T_s/(\tau +T_s)=0.05$ ，控制器实现偏航角解算的滤波算法为：
$$\hat{\psi }(k)=0.95\left(\hat{\psi }(k-1)+T_s{w}_{z_{b}m}(k)\right)+0.05{\psi }_m(k)$$

（2）非线性互补滤波器

略

（3）卡尔曼滤波器

留待以后分解……

打公式太累了……

2. 速度估计

华丽丽的的略……以后分解

3. 位置估计

依然是华丽丽的的略……以后分解

小伙伴们不要觉得我敷衍，这些内容都涉及较深的理论含量，写起来费劲、读起来也费劲，咱们稳扎稳打、细水长流。此外这篇文章本来就是写姿态角解算啊啊啊！！目的已经达到了。下面回到我们的小车，我们的目的是测量小车的航向角，从而控制小车走直线和机动转弯。 以下是代码实现。

四. 姿态角测量与滑动平均滤波

以上的理论推导没看懂没有任何关系，丝毫不影响咱们实现航向角测量，因为我直接简单粗暴的用了姿态角传感器，其内部就是用的卡尔曼滤波算法，直接输出姿态角，控制器根本就不用做姿态解算算法了哈哈（感觉好不要脸啊）。反正实用为王，事实上模块化就是趋势，人的精力是有限的，怎么快怎么来。不过从知识体系的完整性来说，建议还是把理论部分弄通。

姿态角传感器用的维特智能的JY901B和十轴惯导JY-GPSIMU，价格分别为100多和500多，这两个器件都能测气压、高度，十轴惯导还带GPS， 后面写四旋翼系列我们依然用这两个器件。使用两个传感器是冗余设计，有的要求高的场合用三个四个的都有。至于怎么去使用多个，是只用一个还是多个数据取平均都可以灵活调整。

JY901B的数据输出是IIC和UART两种方式，十轴惯导只有UART输出，本文分别用IIC和UART2读两个器件的角度数据。另外JY901B最好焊在板子上，或者用引脚对插，总之注意惯性器件安装一定要稳固。

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];
}

有了上面的驱动函数，后面通过调用IICreadBytes(u8 dev, u8 reg, u8 length, u8 *data)就可以读数据了，例如调用IICreadBytes(0x50, 0x34, 24,&chrTemp[0])就可以一次性读出三轴角度、三轴角速度、三轴加速度和三轴磁场了，因为从JY901内存地址0x34开始依次就是加速度、角速度、磁场和姿态角的数据。IICreadBytes(0x50, 0x45, 24,&chrTemp[0])可以读气压和高度。至于内存地址的映射关系你买了传感器后手册会告诉你的。

2. UART读取十轴惯导姿态角

串口的驱动代码在上一节讲解SBUS信号解析的过程中有提到，在上一节创建的uart.c中补充，实现uart2的中断服务程序。十轴惯导按一定频率上传数据（频率可以自己设置，我设置的200Hz）。十轴惯导上传数据的每帧数据11个字节：

Byte1：0x55（固定，指模块到上位机或单片机）
byte2：functionID，定义如下

FunctionID	0x50	0x51	0x52	0x53	0x54	0x55	0x56	0x57	0x58	0x59
数据	时间	加速度	角速度	角度	磁场	端口状态	气压	经纬度	GPS数据	四元数

Byte3~Byte10：八个字节的数据
Byte11：校验位。

建立一个文件HWT905.h，定义一些数据结构，用于存储串口读出来的数据

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

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

建立一个文件HWT905.c，在这里定义一些全局变量，这些变量在串口中断函数里面要用到。

#include "HT905.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文件中添加一个CopeSerial2Data函数，在串口2的中断函数里面调用CopeSerial2Data函数就可以一次将所有数据读到内存中了。

//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:	flag_stime=1;memcpy(&stcTime,&ucRxBuffer[2],8);break;//memcpy为编译器自带的内存拷贝函数，需引用"string.h"，将接收缓冲区的字符拷贝到数据共同体里面，从而实现数据的解析。
			case 0x51:	flag_sacc=1;memcpy(&stcAcc,&ucRxBuffer[2],8);break;
			case 0x52:	flag_sgro=1;memcpy(&stcGyro,&ucRxBuffer[2],8);break;
			case 0x53:	flag_sangle=1;memcpy(&stcAngle,&ucRxBuffer[2],8);break;
			case 0x54:	flag_smag=1;memcpy(&stcMag,&ucRxBuffer[2],8);break;
			case 0x55:	flag_sdstatus=1;memcpy(&stcDStatus,&ucRxBuffer[2],8);break;
			case 0x56:	flag_spress=1;memcpy(&stcPress,&ucRxBuffer[2],8);break;
			case 0x57:	flag_slonlat=1;memcpy(&stcLonLat,&ucRxBuffer[2],8);break;
			case 0x58:	flag_sgpsv=1;memcpy(&stcGPSV,&ucRxBuffer[2],8);break;
			case 0x59:  flag_quat=1;memcpy(&stcQuat,&ucRxBuffer[2],8);break;
		}
		ucRxCnt=0;
	}
}

3. 滑动平均滤波读取角度和角速度

进过以上步骤实现了IIC和UART读取两个器件数据的功能了。但是这还不够，虽然是直接读角度，但也得滤个波好吧，这里用常见的滑动平均滤波，也叫环形滤波器、也叫有限冲击响应滤波……

创建一个sensor.h和sensor.h文件，专门用来处理传感器数据的读取和滤波。sensor.h文件内容如下：

#ifndef __SENSOR_H
#define __SENSOR_H
#include "sys.h"
#include "delay.h"
#include "JY901_REG.h"
#include "JY901_IIC.h"
#include "HT905.h"
#include "adc.h"

void sensor_Init(void);
// 读JY901的角度、角速度、磁场、气压、高度和十轴IMU的角度、角速度
void sensorReadJY901(float *Gyro, float *Acc, float *Mag, float *Angle);
void sensorReadJY901_height(float *height, float *pressure);
void UART_ReadIMU(float *Gyro, float *Acc, float *Mag, float *Angle);
// 下面两个函数通过滑动平均滤波读角度和角速度
void sensorReadAngle(float *Gyro, float *Angle);
void sensorReadHeight(float *air_hight);
#endif

sensor.c内容如下：

#include "sensor.h"
#include "JY901_REG.h"
#include "JY901_IIC.h"
#include "HT905.h"
#include "sbus.h"
#include "stabilizer.h"

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

#if SYSTEM_SUPPORT_OS
#include "includes.h" //os 使用
#endif

// 惯导值滤波参数
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;
u8 count = 0;

float filterHight[10];
float sumHight;
u8 count_hight = 0;

void sensor_Init(void)
{
    
    
    IIC_Init();
}

void sensorReadJY901(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;
}

// 读高度
void sensorReadJY901_height(float *pressure, float *height)
{
    
    
	OS_ERR err;
	CPU_SR_ALLOC();
	unsigned char chrTemp[30];
	OS_CRITICAL_ENTER();
	IICreadBytes(0x50, PressureL,8,&chrTemp[0]);
	OS_CRITICAL_EXIT();
	*pressure =(float) (chrTemp[3] << 24 |chrTemp[2] << 16 |chrTemp[1] << 8 |chrTemp[0]); // 单位 Pa
	*height = (float) (chrTemp[7] << 24 |chrTemp[6] << 16 |chrTemp[5] << 8 |chrTemp[4]);  // 单位 cm
}

//串口读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;
   
}

// 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) 
		sensorReadJY901(gyro, acc, mag, angle);
	if (command[IMU_MODE] == HT905)
		UART_ReadIMU(gyro, acc, mag, angle);
	if (command[IMU_MODE] == JY901_HT905)	
	{
    
    
		sensorReadJY901(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;
}

这里面的核心就是实现void sensorReadAngle(float *Gyro, float *Angle),通过滑动平均滤波取最近的十次数据做平均，作为三轴角度和角速度的值。这个函数就是主函数里的接口，主函数里面只需要调用这个函数就可以读数据了。

void sensorReadAngle(float *Gyro, float *Angle)里有个传感器选择的判断，使用一个遥控器的三段开关可以选择JY901模块、十轴惯导、或者两者取平均，这一部分怎么用可以自己去设计。

五. UCOS-III 传感任务实现

经过第四章稍显漫长的驱动代码之后，下面可以编写应用代码了，在上一篇STM32实现四驱小车（二）通信任务——遥控器SBUS通信的基础上，在main.c里补充sensor_task()

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(10); 
	}
}

这段代码是不是超级好读懂（自我感觉良好~），我所有的命名都是字面意思。在while循环里面一直调用sensorReadAngle(Gyro, Angle)读取角度和角速度（已经经过了滤波处理），然后用读到的值更新当前状态，state是一个结构体，包含了小车的所有状态量。setstate是期望状态的结构体，包括姿态角、速度等，这个值会根据遥控器的摇杆 开关位置来更新。

到这里就实现了UCOS-III操作系统里面的传感任务了，读取到了较为可靠的三轴角度和三轴角速度。这两组值将会在StabilizationTask（姿态控制任务）中大显身手。

这里做下说明，本系列文章笔者重在分享思想、算法，在讲解上会弱化一些基本知识（比如单片机各个外设的原理、单片机编程的基本知识等），在代码的粘贴上会忽视一些底层的驱动代码和无关紧要的部分，事实上上面的代码我都经过删减了，只留下了干货。所以可以说面向的是中高级选手，拿来主义者可以打道回府了，本系列文章不开源，不提供源码，请见谅。