网站备案 空间备案 域名备案,网页设计制作一个网站,微信app下载链接,网站制作大概费用1. 概述
本教程将结合程序代码及CSS控制站工程#xff0c;讲述如何基于PH47代码框架的BBDB固件版本#xff0c;为开发自己的AHRS姿态解算算法提供完善支撑环境#xff0c;以及数据分析手段。
BBDB固件已内置了一套姿态解算算法。对于需要进行AHRS算法开发研究的开发者讲述如何基于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温度x3A储存温度x1 GPS相关数据 数据类型 数据说明 sGps.fix_type uint8_t_ gps锁定类型无单位 sGps.sat_num uint8_t_ 卫星数量无单位 sGps.loc Location 当前定位数据 Location.lngLocation.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?spm1001.2014.3001.5482相关资源PH47: PH47运动控制代码框架.https://gitee.com/ss15/ph47
