Ardupilot Pre-Arm安全檢查程式分析
阿新 • • 發佈：2018-11-11
目錄
摘要
本節主要分析ardupilot多旋翼部分的Pre-Arm 安全檢查程式，歡迎批評指正！！！
第一：Pre-Arm簡介
Ardupilot：ArduCopter多旋翼 韌體內部有一套非常完善的Pre-Arm安全檢查提醒機制，會檢查你的飛行器是否有大量問題,包括各種未校準，已經是否感測器已經出現損壞，當然這解鎖檢查機制也不是百分百可靠的，你可以通過在全部引數列表Arming-check 禁用它。
需要注意的是：
因為飛控韌體版本的不斷更新，有可能一些錯誤資訊會有所改變，可能與本文出現不一致，或者本文沒有說明的錯誤；都是正常情況。可以自行使用搜索引擎搜尋一下
第二：Pre-Arm報錯需知
1.使用前準備
使用GCS地面站（Missionplanner）檢視Pre-Arm錯誤資訊
在閃爍黃色燈時，使用者會無法解鎖，而且解鎖時蜂鳴器也會長鳴2下。此時你必須連線上地面站，才能排除究竟是什麼問題導致了不能解鎖和飛行，一般通常是 感測器未校準 或者出現了失控保護、低壓設定不正確等等，下面會詳細分析每種報錯內容:
使用Pre-Arm資訊分析不能解鎖原因：
（1）飛控板已經連線好遙控器接收機，並 拆除螺旋槳 和動力電池(這個很重要！！！安全第一)
（2）通過USB或者數傳連線上地面站（Mission Planner）
（3）開啟你的遙控器，並試圖解鎖；解鎖通道是：油門最低 (3通道）、YAW最右（4通道）（這個是apm預設的解鎖方式）
（4）此時，在地面站視窗應該可以看見 紅色 的Pre-Arm 錯誤提示， 如果沒有任何提示 ，一般是 解鎖通道不正確 導致飛控無法感知你的解鎖操作；你還可以在地面站 飛行資料 介面-狀態列 點解鎖操作.
2.解鎖失敗的原因
（1）遙控器常見問題
（2）電子羅盤常見問題
（3）GPS常見故障
（4）INS檢查（例如加速計和陀螺儀檢查）
以上主要參考雷迅資料：雷迅Pre-Arm解析
更重要的資料是官網：ardupilot官網Pre-Arm解析，雷迅純屬翻譯
3.解鎖失敗的原因（自己對照官網進行翻譯）
#1解鎖前安全檢查
（Pre-Arm Safety Check）
多旋翼無人機韌體包括一套完整的安全解鎖檢查程式，以防止因一些故障，在無人機解鎖後，導致安全問題；如果在無人機起飛前遇到何種問題，包括感測器沒有校準，配置或感測器資料不良等錯誤情況，都將禁止無人機起飛。這些檢查有助於防止無人機墜毀和飛行。但如果必要的話，當然我們可以通過引數設定，禁用安全檢查設定，但是我個人感覺應該啟用。（翻譯中夾雜著個人理解）
#2採用GCS識別是什麼導致的Pre-Arm報錯
（Recognising which Pre-Arm Check has failed using the GCS）
飛行員應該注意到：如果解鎖時發生安全檢查失敗，飛行員將無法解鎖無人機，同時飛控伴隨有LED閃爍黃色燈。因此應該準確的進行安全失敗檢查：
1.使用USB資料線或數傳模組，將飛行控制器的資料連線到地面站。
2.確保GCS連線到無人機（即在任務平面上，點選右上角的連線”按鈕”。
3.開啟你的無線電遙控器發射機並試圖解鎖無人機（常規程式使用油門拉到最下面向下，偏航最右邊）
4.解鎖檢查失敗的原因將在HUD視窗中以紅色顯示。我們可以通過報錯的原因進行分析什麼原因導致解鎖禁止。
#3錯誤資訊
（Failure messages）
RC故障（即發射機/接收機故障）：
（1）RC not calibrated：無線電校準尚未執行。RC3_MIN和RC3_MAX必須已經從它們的預設值（1100和1900）更改，並且對於通道1到4，MIN值必須是1300或更小，MAX值必須是1700或更高。
（2）氣壓計故障：
Baro not healthy ：BARO不健康，氣壓計感測器報告它是不健康的，這通常是硬體故障的標誌。
Alt disparity :（高度差），氣壓計高度與慣性導航（即機體氣壓計+加速度計）高度估計相差超過2米。該訊息通常是短暫的，並且可以在飛行控制器首次插入時或者如果它接收到劇烈震動（即突然下降）時發生。如果不清楚，加速度計可能需要校準，或者可能存在氣壓計硬體問題。
（3）羅盤故障：
Compass not healthy :羅盤不健康：指南針感測器報告它是不健康的，這是硬體故障的標誌。
Compass not calibrated :羅盤尚未校準。COMPASS_OFS_X、Y、Z引數為零，或者自上次進行羅盤校準以來連線的羅盤的數量或型別已經改變。
Compass offsets too high :主羅盤的偏移長度（即SqRT（x^ 2 +y^ 2 +z ^ 2））大於500。這可能是由於金屬物體太靠近指南針造成的。如果只使用內部羅盤（不推薦），那麼可能只是板中的金屬造成大的偏移，這實際上可能不是問題，在這種情況下，您可能希望禁用羅盤檢查。
Check mag field : 該區域的感測磁場比預期值高35%或更低。預期長度為530，所以＞874或＜185。磁場強度在世界各地都有所不同，但是這些廣泛的限制意味著羅盤校準更有可能沒有計算出好的偏移量，應該重複。
Compasses inconsistent :羅盤方向不一致：內羅盤和外羅盤指向不同的方向（超過45度）。這通常是由外部羅盤方向（即地理，羅盤方向引數）設定不正確引起的。
（4）GPS相關故障：
GPS Glitch :
GPS燈閃爍：GPS燈正在閃爍，並且無人機處於需要GPS模式（即留待模式、PosHold等）和/或啟用圓形柵欄的飛航模式。
Need 3D Fix : GPS沒有3D修復，並且無人機處於GPS模式和/或啟用圓形柵欄的飛航模式。
Bad Velocity :無人機的速度（根據慣性導航系統）在50cm/s以上。可能導致這種情況的問題包括車輛實際移動或下降，不良的加速度計校準，GPS更新低於預期的5hz。
High GPS HDOP :**GPS的HDPP值（位置精度的值）高於2.0，並且車輛處於GPS定位模式和/或啟用圓形柵欄的飛航模式。我們可以簡單地等待幾分鐘、移動到具有更好的天空檢視的位置或檢查GPS干擾源（即FPV裝置）從GPS移動得更遠來解決。或者，可以通過修改增加**GPS_HDOP_GOOD引數為2.2或2.5來放鬆檢查。最糟糕的情況是，飛行員在不需要GPS(即，穩定，AltHold)的模式下可以禁用圍欄和起飛，並且在解鎖後切換到Loiter，但這不被推薦。（太危險，一定要先進行GPS定位後，在起飛，這個時候三種模式可以選擇）
**注意：**GPS HDOP可以通過任務規劃人員的快速選項卡方便地檢視，如下所示。
（5）INS檢查（即加速度和陀螺檢查）：
**INS not calibrated:**INS未校準：部分或全部加速度計的偏移量為零。加速度計需要校準。
Accels not healthy: ACCEL不健康：一個加速度計報告是不健康的，這可能是硬體問題。這也可以在重新啟動板之前的韌體更新之後立即發生。
Accels inconsistent: 加速度不一致：加速度計報告當前加速度至少為1M/s/s不同。加速度計需要重新校準或存在硬體問題。
Gyros not healthy: 陀螺儀不健康：一個陀螺儀報告它是不健康的，這可能是硬體問題。這也可以在重新啟動板之前的韌體更新之後立即發生。
Gyro cal failed: 陀螺儀校準失敗：陀螺儀校準未能捕獲偏移量。這是經常由無人機在陀螺儀校準期間移動（當紅色和藍色燈閃爍），在這種情況下，拔出電池，並再次插入，而小心不要移動無人機，可能會解決這個問題。感測器硬體故障（即尖峰）也會導致這種故障。
Gyros inconsistent: 陀螺儀不一致：兩個陀螺儀報告的車輛旋轉速率相差20dg/s以上。這可能是硬體故障或由陀螺儀校準不良造成的。
（6）供電檢查：
Board Voltage checks:檢查供電板電壓：電路板內部電壓低於4.3伏或高於5.8伏。
如果通過USB資料線供電（即在工作臺上），這可能是由於臺式計算機無法提供足夠的電流給飛行控制器-嘗試更換USB資料線。
如果由電池供電，這是一個嚴重的問題，電力系統（即電源模組、電池等）應該在飛行前仔細檢查。
（7）引數檢查：
Ch7&Ch8 Opt cannot be same：
Ch7&Ch8選項不能相同：輔助功能開關被設定為相同的選項，這是不允許的，因為它可能導致混亂。
Check FS_THR_VALUE: 遙控器故障安全PWM值，已被設定得太接近節氣門通道（即CH3）最小值。
Check ANGLE_MAX:控制無人機的最大傾角的AGLE_MAX引數被設定在10度（即1000）以下或80度（即8000）以上。
**ACRO_BAL_ROLL/PITCH:**ACRO_BAL_ROLL引數高於穩定輥P和/或ACRO_BAL_PITCH引數高於穩定螺距P值。這可能導致操作者在ACRO模式下無法控制傾角，因為Acro訓練器的穩定將超過操作者的輸入。
第三：Pre-Arm報錯程式碼分析
1.Pre-Arm檢測初始化
void Copter::init_ardupilot()
{
arming.pre_arm_rc_checks(true);
if (ap.pre_arm_rc_check) //這裡只有遙控器檢測過了，才能使能電機輸出
{
enable_motor_output(); //使能電機輸出
}
}（1）arming.pre_arm_rc_checks(true);
執行預先與GPS相關的檢查，如果通過，返回TRUE
void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure)
{
// exit immediately if we've already successfully performed the pre-arm rc check
if (copter.ap.pre_arm_rc_check) {
return;
}
// set rc-checks to success if RC checks are disabled
if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) {
set_pre_arm_rc_check(true);
return;
}
const RC_Channel *channels[] =
{
copter.channel_roll,
copter.channel_pitch,
copter.channel_throttle,
copter.channel_yaw
};
const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" };
for (uint8_t i=0; i<ARRAY_SIZE(channels);i++)
{
const RC_Channel *channel = channels[i];
const char *channel_name = channel_names[i];
// check if radio has been calibrated
if (!channel->min_max_configured()) {
if (display_failure) {
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: RC %s not configured", channel_name);
}
return;
}
if (channel->get_radio_min() > 1300) {
if (display_failure) {
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name);
}
return;
}
if (channel->get_radio_max() < 1700) {
if (display_failure) {
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name);
}
return;
}
if (i == 2) {
// skip checking trim for throttle as older code did not check it
continue;
}
if (channel->get_radio_trim() < channel->get_radio_min()) {
if (display_failure) {
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim below min", channel_name);
}
return;
}
if (channel->get_radio_trim() > channel->get_radio_max()) {
if (display_failure) {
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim above max", channel_name);
}
return;
}
}
// if we've gotten this far rc is ok
set_pre_arm_rc_check(true);
}
（2）enable_motor_output(); //使能電機輸出，當ap.pre_arm_rc_check=1，進行使能，否則不進行
void Copter::enable_motor_output()
{
// enable motors
motors->enable();
motors->output_min();
}2.Pre-Arm檢測更新
（1）第一步函式
SCHED_TASK(one_hz_loop, 1, 100), //進行解鎖檢查（2）第二步函式
void Copter::one_hz_loop()
{
arming.update();
}（3）第三步函式
執行解鎖前的檢查，執行週期是1s，頻率是1Hz
void AP_Arming_Copter::update(void)
{
//每30秒執行一次手臂檢查和顯示失敗資訊---- perform pre-arm checks & display failures every 30 seconds
static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; //# define PREARM_DISPLAY_PERIOD 30
pre_arm_display_counter++;
bool display_fail = false;
if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) //到30s時
{
display_fail = true;
pre_arm_display_counter = 0;
}
if (pre_arm_checks(display_fail)) //重點分析
{
set_pre_arm_check(true);
}
}分析函式pre_arm_checks(display_fail)
bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
{
//立即退出，如果手臂解鎖-----exit immediately if already armed
if (copter.motors->armed())
{
return true;
}
// check if motor interlock and Emergency Stop aux switches are used
// at the same time. This cannot be allowed.
//檢查電機聯鎖和緊急停止輔助開關是否同時使用。這是不允許的。
if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict");
}
return false;
}
//檢查電機聯鎖開關是否使用----check if motor interlock aux switch is in use
//如果是使用了，開關需要撥到不使能位置到解鎖---if it is, switch needs to be in disabled position to arm
//否則立即退出。這個檢查要重複，因為狀態隨時都可以改變。otherwise exit immediately. This check to be repeated, as state can change at any time.
if (copter.ap.using_interlock && copter.ap.motor_interlock_switch)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
}
return false;
}
//如果我們已經成功地進行了預檢查，立即退出---exit immediately if we've already successfully performed the pre-arm check
if (copter.ap.pre_arm_check)
{
//執行GPS檢查，因為結果可能會改變和影響LED顏色不需要顯示失敗，因為如果飛行員嘗試ARMARCHECK將執行該操作。
// run gps checks because results may change and affect LED colour
// no need to display failures because arm_checks will do that if the pilot tries to arm
pre_arm_gps_checks(false);
return true;
}
//返回1，如果解鎖檢查沒有使能-----succeed if pre arm checks are disabled
if (checks_to_perform == ARMING_CHECK_NONE)
{
set_pre_arm_check(true);
set_pre_arm_rc_check(true);
return true;
}
return barometer_checks(display_failure) //氣壓計檢查函式
& rc_calibration_checks(display_failure) //遙控器檢測函式
& compass_checks(display_failure) //檢查羅盤
& gps_checks(display_failure) //檢查gps
& fence_checks(display_failure) //檢查圍欄
& ins_checks(display_failure) //檢查慣性導航資料
& board_voltage_checks(display_failure) //檢查電壓板
& logging_checks(display_failure) //檢查log
& parameter_checks(display_failure) //檢查引數
& motor_checks(display_failure) //檢查電機
& pilot_throttle_checks(display_failure); //檢查飛行油門
}
分析函式set_pre_arm_check(true)
void AP_Arming_Copter::set_pre_arm_check(bool b)
{
if(copter.ap.pre_arm_check != b)
{
copter.ap.pre_arm_check = b;
AP_Notify::flags.pre_arm_check = b;
}
}3.Pre-Arm各個函式分析
1、氣壓計解鎖檢查
bool AP_Arming_Copter::barometer_checks(bool display_failure)
{
//檢查氣壓計------check Baro
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO))
{
//氣壓計是否健康------barometer health check
if(!barometer.healthy())
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy"); //氣壓計不正常
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
//如果EKF執行在一個絕對位置控制模式下，檢查BARO和IANV ALT差是否在1M以內。
//不要檢查是否打算在地面相對高度模式下操作，因為EKF輸出，可能由於氣壓漂移而與氣壓高度在不同的地面相對高度。
nav_filter_status filt_status = _inav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref)
{
if (fabsf(_inav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) //慣性導航獲取高度大於氣壓計的高度1m嗎
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); //報錯高度差，太大
}
return false; //返回0，失敗檢測
}
}
}
return true; //到這裡就成功了
}2、遙控器解鎖檢查
bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
{
//解鎖前，預先進行遙控器錯誤檢查pre-arm rc checks a prerequisite
pre_arm_rc_checks(display_failure);
return copter.ap.pre_arm_rc_check;//返回解鎖狀態
}分析其中的函式： pre_arm_rc_checks(display_failure)
void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure)
{
//立即退出，如果我們已經成功地執行了前臂RC檢查------exit immediately if we've already successfully performed the pre-arm rc check
if (copter.ap.pre_arm_rc_check) //copter.ap.pre_arm_rc_check=1,檢查成功
{
return;
}
//如果RC檢查被禁用，請將RC檢查設定為成功------------set rc-checks to success if RC checks are disabled
if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) //ARMING_CHECK_ALL=0x01,ARMING_CHECK_ALL位標誌
{
set_pre_arm_rc_check(true);
return;
}
const RC_Channel *channels[] = //設定遙控器通道對映，一般設定橫滾-1，俯仰-2，油門-3，偏航-4
{
copter.channel_roll,
copter.channel_pitch,
copter.channel_throttle,
copter.channel_yaw
};
const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" };//字串常量
for (uint8_t i=0; i<ARRAY_SIZE(channels);i++) //計算位元組數
{
const RC_Channel *channel = channels[i]; //獲取通道
const char *channel_name = channel_names[i]; //獲取名字
//檢查遙控器是否校準------------------check if radio has been calibrated
if (!channel->min_max_configured()) //獲取channel->min_max_configured()=1，已經配置，不進入if,否則進入if,會提示錯誤
{
if (display_failure) //display_failure=1
{
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: RC %s not configured", channel_name); //傳送錯誤到地面站
}
return;
}
if (channel->get_radio_min() > 1300) //最小值大於1300嗎，大於將會報錯
{
if (display_failure)
{
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name);
}
return;
}
if (channel->get_radio_max() < 1700) //最大值是否小於1700，小於將會報錯
{
if (display_failure)
{
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name);
}
return;
}
if (i == 2)
{
//當舊程式碼沒有檢查時，跳過油門檢查。----skip checking trim for throttle as older code did not check it
continue;
}
if (channel->get_radio_trim() < channel->get_radio_min()) //遙控器資料，小於最小值，提示太低
{
if (display_failure)
{
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim below min", channel_name);
}
return;
}
if (channel->get_radio_trim() > channel->get_radio_max()) //遙控器資料，大於最大值，提示太大
{
if (display_failure)
{
copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim above max", channel_name);//這個函式後面分析
}
return;
}
}
//如果程式碼跑到這裡，RC就可以使用了。也就是質檢通過，放心使用------ if we've gotten this far rc is ok
set_pre_arm_rc_check(true);//關鍵是設定：copter.ap.pre_arm_rc_check =1
}
分析其中的函式： copter.gcs_send_text_fmt
向GCS傳送低優先順序格式化訊息，佇列中只有一個適合，所以如果在最後一個進入序列緩衝區之前傳送多個訊息，那麼舊的訊息將丟失。這個函式在GCS_Mavlink.cpp中，該函式現在不作分析，知道是通過mavlink傳送錯誤到地面站就可以,,有時間會繼續分析這個函式。
void Copter::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
{
char str[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
va_list arg_list;
va_start(arg_list, fmt);
va_end(arg_list);
hal.util->vsnprintf((char *)str, sizeof(str), fmt, arg_list);
gcs().send_statustext(severity, 0xFF, str);
notify.send_text(str);//通知傳送資訊
}3、羅盤解鎖檢查
bool AP_Arming_Copter::compass_checks(bool display_failure)
{
bool ret = AP_Arming::compass_checks(display_failure);//檢查羅盤解鎖
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS))
{
// check compass offsets have been set. AP_Arming only checks
// this if learning is off; Copter *always* checks.
//檢查羅盤偏移已經被設定。只有當學習結束時，解鎖檢查僅檢查這一點；COPTER總是檢查。
if (!_compass.configured())
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated"); //傳送沒有校準
}
ret = false;
}
}
return ret;
}分析函式AP_Arming::compass_checks(display_failure)
bool AP_Arming::compass_checks(bool report)
{
if ((checks_to_perform) & ARMING_CHECK_ALL ||(checks_to_perform) & ARMING_CHECK_COMPASS) //是否開始羅盤檢查
{
if (!_compass.use_for_yaw()) //如果羅盤使用偏航計算，返回1
{
//如果進入這裡，說明羅盤沒有使能-----compass use is disabled
return true;
}
if (!_compass.healthy()) //判斷羅盤健康嗎_compass.healthy()=1，是健康的，否則是不健康的，會在地面站提示
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass not healthy");//提示錯誤
}
return false;
}
//檢查羅盤正在學習或者偏移已經設定-------- check compass learning is on or offsets have been set
if (!_compass.learn_offsets_enabled() && !_compass.configured())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass not calibrated");
}
return false;
}
//檢查羅盤是否校準------check if compass is calibrating
if (_compass.is_calibrating())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Arm: Compass calibration running");
}
return false;
}
//報告地面站檢查羅盤已經校準完成，需要復位--------check if compass has calibrated and requires reboot
if (_compass.compass_cal_requires_reboot())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
}
return false;
}
//檢查不合理的羅盤偏移量------ check for unreasonable compass offsets
Vector3f offsets = _compass.get_offsets();
if (offsets.length() > _compass.get_offsets_max())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Compass offsets too high");
}
return false;
}
//檢查不合理的MAG欄位長度------ check for unreasonable mag field length
float mag_field = _compass.get_field().length();
if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN)
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Check mag field");
}
return false;
}
//檢查所有羅盤點大致在相同的方向-------check all compasses point in roughly same direction
if (!_compass.consistent())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,"PreArm: Compasses inconsistent");
}
return false;
}
}
return true;
}4、GPS解鎖檢查
bool AP_Arming_Copter::gps_checks(bool display_failure)
{
//檢查GPS-----check GPS
if (!pre_arm_gps_checks(display_failure))
{
return false;
}
return true;
}分析函式pre_arm_gps_checks(display_failure)
bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure)
{
//總是檢查，如果慣性導航已經開始和對資料進行讀取------always check if inertial nav has started and is ready
if (!ahrs.healthy()) //資料不健康
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks"); //等待慣性導航檢查
}
return false;
}
//檢查飛航模式是否需要GPS-----check if flight mode requires GPS
bool mode_requires_gps = copter.mode_requires_GPS(copter.control_mode);
//檢查圍欄是否需要GPS------- check if fence requires GPS
bool fence_requires_gps = false;
#if AC_FENCE == ENABLED
//如果啟用圓形或多邊形柵欄，我們需要GPS。---- if circular or polygon fence is enabled we need GPS
fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
#endif
//如果不需要GPS返回真----- return true if GPS is not required
if (!mode_requires_gps && !fence_requires_gps)
{
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
//確保GPS是好的-------ensure GPS is ok
if (!copter.position_ok())
{
if (display_failure)
{
const char *reason = ahrs.prearm_failure_reason();
if (reason)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason);
} else
{
if (!mode_requires_gps && fence_requires_gps)
{
//在非GPS飛航模式下向使用者說明為什麼需要GPS---- clarify to user why they need GPS in non-GPS flight mode
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Fence enabled, need 3D Fix");
} else
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix");
}
}
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
//檢查GPS故障（如EKF報告）----- check for GPS glitch (as reported by EKF)
nav_filter_status filt_status;
if (_ahrs_navekf.get_filter_status(filt_status))
{
if (filt_status.flags.gps_glitching) {
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: GPS glitching");
}
return false;
}
}
//檢查EKF羅盤方差低於故障安全閾值------ check EKF compass variance is below failsafe threshold
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
_ahrs_navekf.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (mag_variance.length() >= copter.g.fs_ekf_thresh)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance");
}
return false;
}
//檢查home點和EKF點不是太遠----- check home and EKF origin are not too far
if (copter.far_from_EKF_origin(ahrs.get_home()))
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance");
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
//如果禁用GPS檢查，立即返回true----- return true immediately if gps check is disabled
if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS))
{
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
//警告hdop分離---以防止使用者混亂沒有gps鎖定------warn about hdop separately - to prevent user confusion with no gps lock
if (copter.gps.get_hdop() > copter.g.gps_hdop_good)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); //gps精度因子
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
//呼叫父類進行GPS檢查-----call parent gps checks
if (!AP_Arming::gps_checks(display_failure))
{
return false;
}
//如果我們來到這裡，一切都準備OK----- if we got here all must be ok
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}5、圍欄解鎖檢查
bool AP_Arming_Copter::fence_checks(bool display_failure)
{
#if AC_FENCE == ENABLED
//檢查柵欄是否使能-------check fence is initialised
const char *fail_msg = nullptr;
if (!copter.fence.pre_arm_check(fail_msg))
{
if (display_failure && fail_msg != nullptr)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", fail_msg);
}
return false;
}
#endif
return true;
}分析函式copter.fence.pre_arm_check(fail_msg)
bool AC_Fence::pre_arm_check(const char* &fail_msg) const
{
fail_msg = nullptr;
//如果不啟用或不設定柵欄總是返回真------ if not enabled or not fence set-up always return true
if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE)
{
return true;
}
// 檢查沒有限制目前違反-----check no limits are currently breached
if (_breached_fences != AC_FENCE_TYPE_NONE)
{
fail_msg = "vehicle outside fence";
return false;
}
//如果啟用水平限制，檢查慣性導航單元位置是否正常。---- if we have horizontal limits enabled, check inertial nav position is ok
if ((_enabled_fences & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON))>0 &&
!_inav.get_filter_status().flags.horiz_pos_abs && !_inav.get_filter_status().flags.pred_horiz_pos_abs)
{
fail_msg = "fence requires position";
return false;
}
//到這裡一切OK--------if we got this far everything must be ok
return true;
}6、慣性導航資料解鎖檢查
bool AP_Arming_Copter::ins_checks(bool display_failure)
{
bool ret = AP_Arming::ins_checks(display_failure);//檢查慣性導航資料
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) //使能標誌位
{
//獲取EKF態度（如果壞的話，通常是陀螺儀偏差）-----get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) //執行姿態檢查
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling");
}
ret = false;
}
}
return ret;
}
分析函式AP_Arming::ins_checks(display_failure)
bool AP_Arming::ins_checks(bool report)
{
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_INS))
{
const AP_InertialSensor &ins = ahrs.get_ins();
if (!ins.get_gyro_health_all()) //陀螺儀不健康
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not healthy");
}
return false;
}
if (!ins.gyro_calibrated_ok_all()) //陀螺儀沒有校準
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Gyros not calibrated");
}
return false;
}
if (!ins.get_accel_health_all()) //加速度不健康
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Accels not healthy");
}
return false;
}
if (!ins.accel_calibrated_ok_all()) //加速度需要3D校準
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: 3D Accel calibration needed");
}
return false;
}
//檢查加速度計是否已校準並需要重新啟動-------check if accelerometers have calibrated and re