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, TRACEalgo._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