实例教程：BBDB为AHRS算法开发提供完善的支撑环境（下）
4. BBDB基础上为算法实现提供支撑方式二：算法层实现
第2节内容当中讲述在应用层实现AHRS的步骤，这种方法优点在于实现方法比较简单明了。但是缺点在于程序代码的组织算不上很好，AHRS算法代码与PH47框架代码间存在一定的耦合度，会对将来算法代码移植或复用造成一些麻烦。同时也容易对将来在应用层实现其他控制逻辑产生混淆。故不太推荐在应用层实现AHRS算法，而是建议采用本节即将讲述的在算法层实现只有的AHRS算法。
算法层实现AHRS算法的基本方法是将算法实现封装成一个类，一个从姿态解算抽象类基类当中派生子类。这样做的好处在于对算法的封装比较完善，同时基于抽象类多态机制，可以方便的将BBDB固件中预置的姿态解算模块进行替换；或者与其并行运行。以下就实现步骤进行具体讲述：
4.1. 创建姿态解算抽象类的派生子类CAttiCore_Extend
在\DevStudio\Algorithms\文件夹中分别创建派生类CAttiCore_Extend的.h及.cpp文件实现派生类，我们即将在该类当中实现自己的AHRS解算算法。
AttiCore_Extend.h文件：
#ifndef __ATTI_CORE_EXTEND_H__
#define __ATTI_CORE_EXTEND_H__
//
// AttiCore_Extend.h
// 一般用户代码所必须包含的头文件
#include "../CommonDef.h"
#include "../Frame/GlobalFun.h"
#include "AttitudeCore_Port.h" // 姿态解算抽象类头文件
class CAttiCore_Extend : public CAttitudeCore_Port
{
public:
CAttiCore_Extend();
virtual ~CAttiCore_Extend() {};
virtual void FstRunInit(); // AHRS 初始化函数
virtual void Update(const float fDt_ms); // AHRS 算法实现函数
private:
};
#endif
AttiCore_Extend.cpp文件：
//
// AttiCore_Extend.cpp
#include "AttiCore_Extend.h"
CAttiCore_Extend::CAttiCore_Extend()
{
}
void CAttiCore_Extend::FstRunInit()
{
// AHRS 初始化函数
// ……
}
void CAttiCore_Extend::Update(const float fDt_ms)
{
// AHRS 算法实现函数
// ……
}
4.2. 应用层实现自己的AHRS算法（并行运行）
使用C++抽象类多态机制可以将自己AHRS算法封装成类，从而实现功能模块化。更重要的是，使用多态机制不仅可以用自己的AHRS计算模块替换掉PH47框架的置模块，还可以将自己的计算模块与预制模块并行运行。以下就首先讲述预置模块与自有模块并行运行的实现步骤：
4.2.1 在应用层加入对CAttiCore_Extend的文件包含
在文件DevStudio\Algorithms\Algorithms.h中加入文件包含：
#include "./AttiCore_Extend.h"4.2.2 声明指向抽象类基类的指针_pAttitudeCoreExtend
在文件Algorithms.h声明抽象类指针_pAttitudeCoreExtend：
CAttitudeCore_Port *_pAttitudeCoreExtend;4.2.3 实例化指针_pAttitudeCoreExtend
在文件Algorithms.cpp的CAlgorithms.Init()函数中将指向CAttiCore_Extend派生类子类对象的指针赋值给抽象类基类指针_pAttitudeCoreExtend。之后通过该指针来调用CAttiCore_Extend子类成员函数。
_pAttitudeCoreExtend = new CAttiCore_Extend();
ASSERT(_pAttitudeCoreExtend);4.2.4 通过_pAttitudeCoreExtend进行AHRS算法的初始化及调用运行
在算法层CAlgorithms.Init()函数，或是在应用层CAppBBDB.Init() 函数中对AHRS算法类进行初始化；
void CAppBBDB::Init()
{
// 本函数为PH7 框架完全启动后的的初始化函数，可安全使用 ASSERT, TRACE
algo._pAttitudeCoreExtend->FstRunInit();
}在应用层CAppBBDB.FastThread_1000Hz()函数，或NormalThread_250Hz()函数中对AHRS算法类计算函数进行调用。
void CAppBBDB::FastThread_1000Hz()
{
// fRawGyrDt为总线被设置的时间差
float fRawGyrDt_ms = bus.sImu.GyrRaw.GetDt2Prev_us()/1000.0f;
algo._pAttitudeCoreExtend->Update(fRawGyrDt_ms);
}
同样，算法层初始化函数CAlgorithms.Init()运行早于FastThread_1000Hz()、ANormalThread_250Hz()函数。上述工作完成后，自有的AHRS解算类CAttiCore_Extend即可与框架预设姿态解算类CAttiCore_Ecf并行运行。
4.3. 使用自己的AHRS模块替换框架已有的预置模块
替换已有预置模块方法与4.2节高度类似，都是应用了C++抽象类的多态机制，只是替换预置模块在实现上更为简单，步骤如下：
将指向姿态解算抽象类基类的指针，从原有预置指向CAttiCore_Ecf姿态解算类对象指针，更改为指向自有AHRS解算类CAttiCore_Extend的对象指针即可，其余相关函数调用工作框架已经完成无需自己事项。
// _pAttitudeCore = new CAttiCore_Ecf(); // 更改前
_pAttitudeCore= new CAttiCore_Extend(); // 替换为自有解算模块
ASSERT(__pAttitudeCore);
更多内容见CSDN博客专栏：无人机飞控
https://blog.csdn.net/ss15/category_9690939.html?spm=1001.2014.3001.5482
相关资源：PH47: PH47运动控制代码框架.https://gitee.com/ss15/ph47