实例教程：BBDB为AHRS算法开发提供完善的支撑环境（上）

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
备注： 以上数据帧仅为BBDB固件默认的下行数据，若需要增加新的下行数据，可使用通用数据帧USER_DEF_DATA(0x7a)或COMMAND_LONG(0x4c)进行下行传输 上述数据帧当中所包含的字段定义，可参见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