傳媒公司網(wǎng)站建設(shè)方案百度關(guān)鍵詞排名靠前
1. 概述
本教程將結(jié)合程序代碼及CSS控制站工程，講述如何基于PH47代碼框架的BBDB固件版本，為開發(fā)自己的AHRS姿態(tài)解算算法提供完善支撐環(huán)境，以及數(shù)據(jù)分析手段。
BBDB固件已內(nèi)置了一套姿態(tài)解算算法。對于需要進行AHRS算法開發(fā)研究的開發(fā)者，可在BBDB已有AHRS算法基礎(chǔ)上另行增加自己的算法進行性能比對，或使用自己的算法模塊替換預(yù)置AHRS解算模塊。
本節(jié)教程就在PH47代碼框架的應(yīng)用層實現(xiàn)AHRS算法開發(fā)環(huán)境，以及如何使用CSS控制站工程顯示、分析、記錄相關(guān)算法數(shù)據(jù)進行講述。關(guān)于在算法層實現(xiàn)對AHRS算法的支撐，以及如何使用自己的AHRS算法替換BBDB預(yù)置的算法模塊，將在下節(jié)教程講述。
2. BBDB基礎(chǔ)上為算法實現(xiàn)提供支撐方式一：應(yīng)用層實現(xiàn)
2.1. 代碼初始化與解算代碼的實現(xiàn)位置
通常我們的算法都以模塊化方式實現(xiàn)，既方便程序設(shè)計，又利于將來代碼復(fù)用。算法模塊能夠以一個或多個C++函數(shù)的方式進行組織，也能夠通過封裝為C++類的方式進行實現(xiàn)。
基于PH47代碼框架BBDB固件版本，用戶的AHRS算法模塊既可以簡便的在的應(yīng)用層（Application）實現(xiàn)；又可以進行更好的封裝后在算法層（Algorithms）中實現(xiàn)。本節(jié)就首先對在應(yīng)用層的實現(xiàn)方式進行講解。
此方式下，用戶的AHRS代碼可在BBDB的應(yīng)用層實現(xiàn)類CAppBBDB（\DevStudio\Application\App_BBDB.cpp/.h）中實現(xiàn)或調(diào)用。具體如下：
| 函數(shù)名稱 | AHRS代碼實現(xiàn) |
| CAppBBDB. Init() | PH47框架初始化函數(shù)。 用于AHRS算法初始化功能實現(xiàn)。本函數(shù)在PH47代碼框架初始化過程調(diào)用，可安全使用PH47的各項調(diào)試或其他功能函數(shù)。 注意： 在本函數(shù)調(diào)用之前快速線程函數(shù)如FastThread_1000Hz(), NormalThread_250Hz() 已經(jīng)開始運行，所以位于上述函數(shù)中的代碼應(yīng)對初始化完成情況進行判斷（借助標志變量core.blSysInitCompleted或全局狀態(tài)變量S_INIT_COMPLETE） |
| CAppBBDB. FastThread_1000Hz() | PH47框架快速線程函數(shù)。 可用于AHRS解算代碼的具體實現(xiàn)。根據(jù)控制板mcu不同型號，調(diào)用頻率在400-500hz間波動?？赏ㄟ^相關(guān)函數(shù)獲取兩次調(diào)用間的時間間隔（后面詳述）。 注意： AHRS解算時間必須小于函數(shù)調(diào)用周期（2-2.5ms），事實上，由于在該線程中還有框架自身的函數(shù)運行，故實際解算時間要更短。 關(guān)于本線程全部的運行耗時，可通過在調(diào)試串口輸入”freemem;”命令進行查詢。 |
| CAppBBDB. NormalThread_250Hz() | PH47框架普通線程函數(shù)。 可用于AHRS解算代碼的具體實現(xiàn)。調(diào)用頻率固定為250hz。 關(guān)于對AHRS運行時間的要求與FastThread_1000Hz()函數(shù)相同。 |
2.2. AHRS相關(guān)的輸入數(shù)據(jù)
AHRS算法部分以模塊化方式實現(xiàn)，那么模塊的輸入輸出數(shù)據(jù)交換就是必要的一個環(huán)節(jié)。PH47代碼框架以數(shù)據(jù)總線、全局狀態(tài)變量、機載控制參數(shù)3種不同的形式將相關(guān)的數(shù)據(jù)提供給各功能模塊使用。
除了框架提供的數(shù)據(jù)之外，用戶還可以根據(jù)自己的需要，向數(shù)據(jù)總線、全局狀態(tài)變量、機載控制參數(shù)中加入自己定制的數(shù)據(jù)。
與AHRS解算模塊的相關(guān)輸入數(shù)據(jù)如下（如無特殊說明，數(shù)據(jù)單位均為國際標準單位，以下均同）：
| IMU相關(guān)數(shù)據(jù) | 數(shù)據(jù)類型 | 數(shù)據(jù)說明 |
| 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溫度數(shù)據(jù)（Gyro溫度x3，A儲存溫度x1） |
| GPS相關(guān)數(shù)據(jù) | 數(shù)據(jù)類型 | 數(shù)據(jù)說明 |
| sGps.fix_type | uint8_t_ | gps鎖定類型（無單位） |
| sGps.sat_num | uint8_t_ | 衛(wèi)星數(shù)量（無單位） |
| sGps.loc?? | Location | 當前定位數(shù)據(jù)， 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 |
| 全局狀態(tài)變量 | 數(shù)據(jù)說明 |
| S_IMU_ENABLE | IMU初始化工作完成標志，true表示初始化成功 |
| S_MAG_ENBALE | 磁強計初始化標志，true表示初始化完成 |
| S_COMPASS_BE_USED | 磁強計數(shù)據(jù)有效標志 true表示根據(jù)磁強數(shù)據(jù)計算的航向角誤差在允許范圍內(nèi) |
2.3. AHRS解算后輸出數(shù)據(jù)
經(jīng)AHRS解算后的數(shù)據(jù)同樣可以返回到數(shù)據(jù)總線、全局狀態(tài)變量、機載控制參數(shù)當中。框架中與AHRS輸出相關(guān)的預(yù)置數(shù)據(jù)如下：
| AHRS輸出數(shù)據(jù) | 數(shù)據(jù)類型 | 數(shù)據(jù)說明 |
| AngleRate | Vector3f | 機體平臺三軸轉(zhuǎn)動角速率 |
| AccelBody | Vector3f | 機體坐標系三軸加速度 |
| AccelNed | Vector3f | North-East-Down坐標系三軸加速度 |
| sAhrs.Roll | float | 機體滾轉(zhuǎn)角 |
| sAhrs.Pitch | float | 機體俯仰角 |
| sAhrs.Yaw | float | 機體航向角 |
| sAhrs.SinRoll | float | 滾轉(zhuǎn)角正弦值 |
| sAhrs.SinPitch | float | 俯仰角正弦值 |
| sAhrs.SinYaw | float | 航向角正弦值 |
| sAhrs.CosRoll | float | 滾轉(zhuǎn)角余弦值 |
| sAhrs.CosPitch | float | 俯仰角余弦值 |
| sAhrs.CosYaw | float | 航向角余弦值 |
| sAhrs.dcm | float | 方向余弦矩陣 |
2.4. 解算頻率，及對數(shù)據(jù)時間間隔的使用
AHRS解算循環(huán)當中的時間間隔數(shù)據(jù)對算法實現(xiàn)非常重要，如前文所述，CAppBBDB.FastThread_1000Hz()函數(shù)運行頻率是一個在1000-1500hz之間波動的一個數(shù)值（波動范圍根據(jù)控制板MCU不同而不同），但實際上，在基于PH47框架的程序開發(fā)中：
強烈不推薦使用循環(huán)函數(shù)的設(shè)計運行頻率來計算循環(huán)時間間隔。
建議使用實際測量的循環(huán)時間間隔，或者使用總線數(shù)據(jù)設(shè)置時間間隔來作為算法設(shè)計所需要的時間間隔。框架提供了一個簡單的類CLoopDt用于循環(huán)時間間隔的測量，示例代碼如下：
// Step1：在.h文件中定義CloopDt類對象 _QuatDt
CloopDt _QuatDt;????????// Step2 在.cpp文件的循環(huán)代碼中獲取當前時刻距離上一次調(diào)用本函數(shù)時刻的
// 時間差，以ms為單位，精確到小數(shù)點后三位
// 如果過獲取的dt為0，則返回 false
if( _QuatDt.GetLocalDt_ms(_fDt_Sec) )_fDt_Sec /= 1000.0f;?????????????? // 將獲取時間間隔從ms轉(zhuǎn)換為sec通過上述代碼即可精確的獲取循環(huán)運行時間間隔。通過此方式獲取的時間間隔，一般稱為本地時間間隔。與之相對應(yīng)的是原始時間間隔。
原始時間間隔是指通過總線數(shù)據(jù)的時間戳計算獲取的時間間隔，因為該時間戳是總線數(shù)據(jù)被設(shè)置時刻記錄的時間（精確到us），故總線數(shù)據(jù)兩次被設(shè)置之間的間隔就稱為原始時間間隔。通過簡單函數(shù)調(diào)用就可獲得總線數(shù)據(jù)的原始時間間隔：
// 獲取陀螺儀原始角速度的設(shè)置時間間隔
float fRawGyrDt_ms = bus.sImu.GyrRaw.GetDt2Prev_us()/1000.0f;??在AHRS算法實現(xiàn)過程中，應(yīng)當仔細和小心的確定在什么時候使用本地時間間隔，在什么時候該使用原始時間間隔。
2.5. 應(yīng)用層代碼框架示例
若我們在應(yīng)用層進行AHRS算法模塊的開發(fā)，那么該模塊的初始化函數(shù)為InitMyAHRS()，在CAppBBDB.Init() 函數(shù)中被調(diào)用。
姿態(tài)解算函數(shù)為UpdateMyAHRS()，在CAppBBDB.FastThread_1000Hz()，或是在CAppBBDB.NormalThread_250Hz()函數(shù)中被調(diào)用。
在App_BBDB.h文件中對函數(shù)進行聲明。當然，AHRS解算必須的一些成員變量也可以在.h文件中進行聲明。
// AHRS 應(yīng)用層框架代碼
void InitMyAHRS();
bool UpdateMyAHRS();// AHRS解算所需的成員變量聲明…
// ...在App_BBDB.cpp文件中實現(xiàn)函數(shù)InitMyAHRS()及UpdateMyAHRS()：
通過上述代碼框架框架實現(xiàn)，用戶即可在上述框架中實現(xiàn)自己的姿態(tài)解算算法，并將于姿態(tài)相關(guān)的各種數(shù)據(jù)，通過如下數(shù)據(jù)幀下行發(fā)送至CSS進行顯示、記錄、回放，以及后續(xù)數(shù)據(jù)分析使用：
| 數(shù)據(jù)幀 (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 |
| 備注：
| ||
void CAppBBDB::InitMyAHRS()
{/******************************************************AHRS 算法初始化函數(shù)本函數(shù)在系統(tǒng)初始化過程中調(diào)用函數(shù)調(diào)用順序: CThreadCtrl_BBDB.InitAfterThreadStart() ==> CAppBBDB.Init()==> CAppBBDB.InitMyAHRS()******************************************************//******************************************************* AHRS 初始化過程實現(xiàn) ...******************************************************/
}bool CAppBBDB::UpdateMyAHRS()
{/******************************************************AHRS 算法實現(xiàn)函數(shù)本函數(shù)在 CAppBBDB.FastThread_1000Hz() 或 CAppBBDB.NormalThread_250Hz() 函數(shù)中被調(diào)用******************************************************/// 若系統(tǒng)初始化(即包含了 InitMyAHRS() 初始化)還未完成，則暫不進行 AHRS 解算過程if(core.blSysInitCompleted == false)return false;// 獲取陀螺儀原始角速度的設(shè)置時間間隔// imu 當中的 AccRaw, GyrRaw 數(shù)據(jù)在 CThreadCtrl_BBDB.IdleLoop() 函數(shù)中被近乎同時的進行設(shè)置(< 1us)// 故可用 AccRaw, GyrRaw 當中的任一原始時間間隔作為這兩個數(shù)據(jù)被設(shè)置的時間間隔 float fDt_ms_RawGyr = bus.sImu.GyrRaw.GetDt2Prev_us() / 1000.0f;Vector3f vGyrRaw = bus.sImu.GyrRaw.Get(); // 獲取陀螺儀原始角速度Vector3f vAccRaw = bus.sImu.AccRaw.Get(); // 獲取加速度計原始數(shù)據(jù)Vector3f vMagAvg = bus.sImu.MagAvg.Get(); // 獲取磁強計測量的平均磁強float fCog = bus.sGps.cog.Get(); // 獲取GPS計算的飛行航跡角// 其他 AHRS 解算所需數(shù)據(jù) ...bool blImuInit = gGetStatus(S_IMU_ENABLE); // 獲取 imu 初始化完成標志// 其他全局狀態(tài)變量獲取 ...float fGyrLPF = core.para.Get(P_SENSOR_GYR_LPF_Hz); // 獲取陀螺儀低通濾波器截止頻率// 其他機載控制參數(shù)獲取 .../******************************************************* ...* AHRS 解算過程實現(xiàn) ...* ...******************************************************//******************************************************* AHRS 輸出數(shù)據(jù)設(shè)置 ...bus.sAhrs.dcm.Set(_dcm);bus.sAhrs.Roll.Set(_euler.x);...******************************************************//******************************************************* 如有必要, 對調(diào)試數(shù)據(jù)賦值 ...bus.arDbg_1[0].Set(_fDbg_x);bus.arDbg_1[6].Set(_fDbg_y);bus.arDbg_2[1].Set(_fDbg_z);******************************************************//******************************************************* 如有必要, 根據(jù) AHRS 解算結(jié)果設(shè)置全局變量 ...gSetStatus(S_MY_STATUS);******************************************************/return true;
}3. 使用CSS的BBDB控制站工程提進行觀測分析
在BBDB固件的支持下，AHRS算法設(shè)計中的相關(guān)數(shù)據(jù)通過前述預(yù)設(shè)數(shù)據(jù)幀下行發(fā)送到地面控制站（CSS），并通過對應(yīng)的控制站工程CssDemo_BBDB進行解析、顯示、記錄，以及后續(xù)的分析。關(guān)于CSS的使用方法，可參見《Control Station Studio控制站開發(fā)平臺概述》。以下僅簡單講述：
3.1. 接收并記錄BBP控制板下行數(shù)據(jù)
選擇“自動記錄”模式后點擊“GCS啟動”啟動CSS，開始接收、解析、顯示、記錄來自于BBP控制板的下行數(shù)據(jù)。
3.2. 使用儀表控件對AHRS數(shù)據(jù)進行顯示
控制站工程CssDemo_BBDB能夠以數(shù)字、姿態(tài)顯示儀表、波形圖等多種方式顯示與AHRS相關(guān)的各種數(shù)據(jù)。若現(xiàn)有控件不足以滿足數(shù)據(jù)觀測需求，可對已有控件的顯示屬性進行修改，或增加新的控件（如增加新的波形圖控件疊加顯示多路數(shù)據(jù)）。
3.3. 對AHRS數(shù)據(jù)進行初步分析
將CSS切換到“數(shù)據(jù)分析”頁面，打開先前的數(shù)據(jù)記錄文件，選擇一個或多個需要進行分析的數(shù)據(jù)（數(shù)據(jù)幀中字段）繪制數(shù)據(jù)曲線供分析使用。在分析過程中，可通過設(shè)定曲線的原點、縮放比例等方式進行輔助。
3.4. 導(dǎo)出記錄數(shù)供進一步分析
切換“數(shù)據(jù)分析”頁面，點擊“導(dǎo)出txt”按鈕，打開對應(yīng)的數(shù)據(jù)記錄文件，選擇需要包含導(dǎo)出數(shù)據(jù)的數(shù)據(jù)幀，選擇確定后即可將控制板記錄數(shù)據(jù)導(dǎo)出為txt格式供進一步分析使用。
更多內(nèi)容見CSDN博客專欄：無人機飛控
https://blog.csdn.net/ss15/category_9690939.html?spm=1001.2014.3001.5482相關(guān)資源：PH47: PH47運動控制代碼框架.https://gitee.com/ss15/ph47