1. 概述
本教程将结合程序代码及CSS控制站工程，讲述如何基于PH47代码框架的BBDB固件版本，为开发自己的AHRS姿态解算算法提供完善支撑环境，以及数据分析手段。
BBDB固件已内置了一套姿态解算算法。对于需要进行AHRS算法开发研究的开发者，可在BBDB已有AHRS算法基础上另行增加自己的算法进行性能比对，或使用自己的算法模块替换预置AHRS解算模块。
本节教程就在PH47代码框架的应用层实现AHRS算法开发环境，以及如何使用CSS控制站工程显示、分析、记录相关算法数据进行讲述。关于在算法层实现对AHRS算法的支撑，以及如何使用自己的AHRS算法替换BBDB预置的算法模块，将在下节教程讲述。
2. BBDB基础上为算法实现提供支撑方式一：应用层实现
2.1. 代码初始化与解算代码的实现位置
通常我们的算法都以模块化方式实现，既方便程序设计，又利于将来代码复用。算法模块能够以一个或多个C++函数的方式进行组织，也能够通过封装为C++类的方式进行实现。
基于PH47代码框架BBDB固件版本，用户的AHRS算法模块既可以简便的在的应用层（Application）实现；又可以进行更好的封装后在算法层（Algorithms）中实现。本节就首先对在应用层的实现方式进行讲解。
此方式下，用户的AHRS代码可在BBDB的应用层实现类CAppBBDB（\DevStudio\Application\App_BBDB.cpp/.h）中实现或调用。具体如下：
|----------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
| 函数名称 | AHRS代码实现 |
| CAppBBDB. Init() | PH47框架初始化函数。 用于AHRS算法初始化功能实现。本函数在PH47代码框架初始化过程调用，可安全使用PH47的各项调试或其他功能函数。 注意： 在本函数调用之前快速线程函数如FastThread_1000Hz(), NormalThread_250Hz() 已经开始运行，所以位于上述函数中的代码应对初始化完成情况进行判断（借助标志变量core.blSysInitCompleted或全局状态变量S_INIT_COMPLETE） |
| CAppBBDB. FastThread_1000Hz() | PH47框架快速线程函数。 可用于AHRS解算代码的具体实现。根据控制板mcu不同型号，调用频率在400-500hz间波动。可通过相关函数获取两次调用间的时间间隔（后面详述）。 注意： AHRS解算时间必须小于函数调用周期（2-2.5ms），事实上，由于在该线程中还有框架自身的函数运行，故实际解算时间要更短。 关于本线程全部的运行耗时，可通过在调试串口输入"freemem;"命令进行查询。 |
| CAppBBDB. NormalThread_250Hz() | PH47框架普通线程函数。 可用于AHRS解算代码的具体实现。调用频率固定为250hz。 关于对AHRS运行时间的要求与FastThread_1000Hz()函数相同。 |
2.2. AHRS相关的输入数据
AHRS算法部分以模块化方式实现，那么模块的输入输出数据交换就是必要的一个环节。PH47代码框架以数据总线、全局状态变量、机载控制参数3种不同的形式将相关的数据提供给各功能模块使用。
除了框架提供的数据之外，用户还可以根据自己的需要，向数据总线、全局状态变量、机载控制参数中加入自己定制的数据。
与AHRS解算模块的相关输入数据如下（如无特殊说明，数据单位均为国际标准单位，以下均同）：
|-------------------|---------------------|---------------------------|
| IMU****相关数据 | 数据类型 | 数据说明 |
| IMU.GyrRaw | Vector3f | 陀螺原始角速度（x,y.z三轴，下同） |
| IMU.GyrFilted | Vector3f | 陀螺滤波后角速度 |
| IMU.AccRaw | Vector3f | 加速度计原始加速度 |
| IMU.AccFilted | Vector3f | 加速度计滤波后加速度 |
| IMU.MagRaw | Vector3f | 磁强计原始磁强（无单位） |
| IMU.MagAvg | Vector3f | 磁强计平均磁强（无单位） |
| IMU.Temp | IMU_TEMPERATURE | IMU温度数据（Gyro温度x3，A储存温度x1） |
|------------------------------|--------------|-------------------------------------------------------------------|
| GPS 相关数据 | 数据类型 | 数据说明 |
| sGps.fix_type | uint8_t_ | gps锁定类型（无单位） |
| sGps.sat_num | uint8_t_ | 卫星数量（无单位） |
| sGps.loc | Location | 当前定位数据， Location.lng，Location.lat单位为1e-7degree， Location.alt单位为mm |
| sGps.vel | Vector3f | 当前gps 3维速度矢量 |
| sGps.eph | float | 垂直定位精度 |
| sGps.epv | float | 水平定位精度 |
| sGps.groundspeed | float | 地速 |
| sGps.speed_acc | float | 速度精度 |
| sGps.p_dop | float | 位置定位精度 |
| sGps.cog | float | gps航迹角 |
| sGps.time_ms | uint32_t | |
| sGps. locked_time_ms | uint32_t | |
| sGps.locked_dt | float | |
|-----------------------|--------------------------------------|
| 全局状态变量 | 数据说明 |
| S_IMU_ENABLE | IMU初始化工作完成标志，true表示初始化成功 |
| S_MAG_ENBALE | 磁强计初始化标志，true表示初始化完成 |
| S_COMPASS_BE_USED | 磁强计数据有效标志 true表示根据磁强数据计算的航向角误差在允许范围内 |
2.3. AHRS解算后输出数据
经AHRS解算后的数据同样可以返回到数据总线、全局状态变量、机载控制参数当中。框架中与AHRS输出相关的预置数据如下：
|--------------------|--------------|-------------------------|
| AHRS 输出数据 | 数据类型 | 数据说明 |
| AngleRate | Vector3f | 机体平台三轴转动角速率 |
| AccelBody | Vector3f | 机体坐标系三轴加速度 |
| AccelNed | Vector3f | North-East-Down坐标系三轴加速度 |
| sAhrs.Roll | float | 机体滚转角 |
| sAhrs.Pitch | float | 机体俯仰角 |
| sAhrs.Yaw | float | 机体航向角 |
| sAhrs.SinRoll | float | 滚转角正弦值 |
| sAhrs.SinPitch | float | 俯仰角正弦值 |
| sAhrs.SinYaw | float | 航向角正弦值 |
| sAhrs.CosRoll | float | 滚转角余弦值 |
| sAhrs.CosPitch | float | 俯仰角余弦值 |
| sAhrs.CosYaw | float | 航向角余弦值 |
| sAhrs.dcm | float | 方向余弦矩阵 |
2.4. 解算频率，及对数据时间间隔的使用
AHRS解算循环当中的时间间隔数据对算法实现非常重要，如前文所述，CAppBBDB.FastThread_1000Hz()函数运行频率是一个在1000-1500hz之间波动的一个数值（波动范围根据控制板MCU不同而不同），但实际上，在基于PH47框架的程序开发中：
强烈不推荐使用循环函数的设计运行频率来计算循环时间间隔。
建议使用实际测量的循环时间间隔，或者使用总线数据设置时间间隔来作为算法设计所需要的时间间隔。框架提供了一个简单的类CLoopDt用于循环时间间隔的测量，示例代码如下：
// Step1：在.h文件中定义CloopDt类对象 _QuatDt
CloopDt _QuatDt;
// Step2 在.cpp文件的循环代码中获取当前时刻距离上一次调用本函数时刻的
// 时间差，以ms为单位，精确到小数点后三位
// 如果过获取的dt为0，则返回 false
if( _QuatDt.GetLocalDt_ms(_fDt_Sec) )
_fDt_Sec /= 1000.0f; // 将获取时间间隔从ms转换为sec
通过上述代码即可精确的获取循环运行时间间隔。通过此方式获取的时间间隔，一般称为本地时间间隔。与之相对应的是原始时间间隔。
原始时间间隔是指通过总线数据的时间戳计算获取的时间间隔，因为该时间戳是总线数据被设置时刻记录的时间（精确到us），故总线数据两次被设置之间的间隔就称为原始时间间隔。通过简单函数调用就可获得总线数据的原始时间间隔：
// 获取陀螺仪原始角速度的设置时间间隔
float fRawGyrDt_ms = bus.sImu.GyrRaw.GetDt2Prev_us()/1000.0f;
在AHRS算法实现过程中，应当仔细和小心的确定在什么时候使用本地时间间隔，在什么时候该使用原始时间间隔。
2.5. 应用层代码框架示例
若我们在应用层进行AHRS算法模块的开发，那么该模块的初始化函数为InitMyAHRS()，在CAppBBDB.Init() 函数中被调用。
姿态解算函数为UpdateMyAHRS()，在CAppBBDB.FastThread_1000Hz()，或是在CAppBBDB.NormalThread_250Hz()函数中被调用。
在App_BBDB.h文件中对函数进行声明。当然，AHRS解算必须的一些成员变量也可以在.h文件中进行声明。
// AHRS 应用层框架代码
void InitMyAHRS();
bool UpdateMyAHRS();
// AHRS解算所需的成员变量声明...
// ...
在App_BBDB.cpp文件中实现函数InitMyAHRS()及UpdateMyAHRS()：
通过上述代码框架框架实现，用户即可在上述框架中实现自己的姿态解算算法，并将于姿态相关的各种数据，通过如下数据帧下行发送至CSS进行显示、记录、回放，以及后续数据分析使用：
|-----------------------|--------------|---------------------------------------------------------------------|
| 数据帧 (mavlink message) | 帧id (msg id) | 包含字段 (Field of mavlink message) |
| GPS_RAW_INT | 0x18 | lat, Lon, alt eph, epv, vel, cog, fix_type, satellites_visi |
| RAW_IMU | 0x1b | xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag |
| ATTITUDE | 0x1e | roll, pitch, yaw, rollspeed, pitchspeed, yawspeed |
| PILOT_DBG_1 | 0xf7 | fvalue_0 -- fvalue_9 |
| PILOT_DBG_2 | 0xf8 | fvalue_0 -- fvalue_9 |
| 备注： 1. 以上数据帧仅为BBDB固件默认的下行数据，若需要增加新的下行数据，可使用通用数据帧USER_DEF_DATA(0x7a) 或**COMMAND_LONG(0x4c)**进行下行传输 2. 上述数据帧当中所包含的字段定义，可参见CSS帧编辑器。 |||
void CAppBBDB::InitMyAHRS()
{
/******************************************************
AHRS 算法初始化函数
本函数在系统初始化过程中调用
函数调用顺序:
CThreadCtrl_BBDB.InitAfterThreadStart() ==> CAppBBDB.Init()
==> CAppBBDB.InitMyAHRS()
******************************************************/
/******************************************************
* AHRS 初始化过程实现 ...
******************************************************/
}
bool CAppBBDB::UpdateMyAHRS()
{
/******************************************************
AHRS 算法实现函数
本函数在 CAppBBDB.FastThread_1000Hz() 或
CAppBBDB.NormalThread_250Hz() 函数中被调用
******************************************************/
// 若系统初始化(即包含了 InitMyAHRS() 初始化)还未完成，则暂不进行 AHRS 解算过程
if(core.blSysInitCompleted == false)
return false;
// 获取陀螺仪原始角速度的设置时间间隔
// imu 当中的 AccRaw, GyrRaw 数据在 CThreadCtrl_BBDB.IdleLoop() 函数中被近乎同时的进行设置(< 1us)
// 故可用 AccRaw, GyrRaw 当中的任一原始时间间隔作为这两个数据被设置的时间间隔
float fDt_ms_RawGyr = bus.sImu.GyrRaw.GetDt2Prev_us() / 1000.0f;
Vector3f vGyrRaw = bus.sImu.GyrRaw.Get(); // 获取陀螺仪原始角速度
Vector3f vAccRaw = bus.sImu.AccRaw.Get(); // 获取加速度计原始数据
Vector3f vMagAvg = bus.sImu.MagAvg.Get(); // 获取磁强计测量的平均磁强
float fCog = bus.sGps.cog.Get(); // 获取GPS计算的飞行航迹角
// 其他 AHRS 解算所需数据 ...
bool blImuInit = gGetStatus(S_IMU_ENABLE); // 获取 imu 初始化完成标志
// 其他全局状态变量获取 ...
float fGyrLPF = core.para.Get(P_SENSOR_GYR_LPF_Hz); // 获取陀螺仪低通滤波器截止频率
// 其他机载控制参数获取 ...
/******************************************************
* ...
* AHRS 解算过程实现 ...
* ...
******************************************************/
/******************************************************
* AHRS 输出数据设置 ...
bus.sAhrs.dcm.Set(_dcm);
bus.sAhrs.Roll.Set(_euler.x);
...
******************************************************/
/******************************************************
* 如有必要, 对调试数据赋值 ...
bus.arDbg_1[0].Set(_fDbg_x);
bus.arDbg_1[6].Set(_fDbg_y);
bus.arDbg_2[1].Set(_fDbg_z);
******************************************************/
/******************************************************
* 如有必要, 根据 AHRS 解算结果设置全局变量 ...
gSetStatus(S_MY_STATUS);
******************************************************/
return true;
}
3. 使用CSS的BBDB控制站工程提进行观测分析
在BBDB固件的支持下，AHRS算法设计中的相关数据通过前述预设数据帧下行发送到地面控制站（CSS），并通过对应的控制站工程CssDemo_BBDB进行解析、显示、记录，以及后续的分析。关于CSS的使用方法，可参见《Control Station Studio控制站开发平台概述》。以下仅简单讲述：
3.1. 接收并记录BBP控制板下行数据
选择"自动记录"模式后点击"GCS启动"启动CSS，开始接收、解析、显示、记录来自于BBP控制板的下行数据。
3.2. 使用仪表控件对AHRS数据进行显示
控制站工程CssDemo_BBDB能够以数字、姿态显示仪表、波形图等多种方式显示与AHRS相关的各种数据。若现有控件不足以满足数据观测需求，可对已有控件的显示属性进行修改，或增加新的控件（如增加新的波形图控件叠加显示多路数据）。
3.3. 对AHRS数据进行初步分析
将CSS切换到"数据分析"页面，打开先前的数据记录文件，选择一个或多个需要进行分析的数据（数据帧中字段）绘制数据曲线供分析使用。在分析过程中，可通过设定曲线的原点、缩放比例等方式进行辅助。
3.4. 导出记录数供进一步分析
切换"数据分析"页面，点击"导出txt"按钮，打开对应的数据记录文件，选择需要包含导出数据的数据帧，选择确定后即可将控制板记录数据导出为txt格式供进一步分析使用。
更多内容见CSDN博客专栏：无人机飞控
https://blog.csdn.net/ss15/category_9690939.html?spm=1001.2014.3001.5482** 相关资源：**************************************************************PH47: PH47运动控制代码框架.https://gitee.com/ss15/ph47****************************************************************