版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lixiaoweimashixiao/article/details/82185581
目录
摘要
本节主要分析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 require reboot
if (ins.accel_cal_requires_reboot())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Accels calibrated requires reboot");
}
return false;
}
//检查所有加速度计点大致相同的方向----check all accelerometers point in roughly same direction
if (ins.get_accel_count() > 1)
{
const Vector3f &prime_accel_vec = ins.get_accel();
for(uint8_t i=0; i<ins.get_accel_count(); i++)
{
if (!ins.use_accel(i))
{
continue;
}
// 获取下一个ACCEL向量----get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
//允许用户定义的差异，通常为0.75米/秒/秒，必须在最后10秒内通过。---allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
float threshold = accel_error_threshold;
if (i >= 2)
{
/*
we allow for a higher threshold for IMU3 as it
runs at a different temperature to IMU1/IMU2,
and is not used for accel data in the EKF
*/
//我们允许更高的阈值IMU3，因为它在不同的温度下运行到IMU1/IMU2，并且不用于EKF中的ACCEL数据。
threshold *= 3;
}
//EKF对Z轴误差不敏感---- EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;
if (vec_diff.length() <= threshold)
{
last_accel_pass_ms[i] = AP_HAL::millis();
}
if (AP_HAL::millis() - last_accel_pass_ms[i] > 10000)
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Accels inconsistent");
}
return false;
}
}
}
//检查所有陀螺仪是否给出一致的读数----check all gyros are giving consistent readings
if (ins.get_gyro_count() > 1)
{
const Vector3f &prime_gyro_vec = ins.get_gyro();
for(uint8_t i=0; i<ins.get_gyro_count(); i++)
{
if (!ins.use_gyro(i))
{
continue;
}
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference. Pass if it has
// been OK in last 10 seconds
if (vec_diff.length() <= radians(5))
{
last_gyro_pass_ms[i] = AP_HAL::millis();
}
if (AP_HAL::millis() - last_gyro_pass_ms[i] > 10000)
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Gyros inconsistent");
}
return false;
}
}
}
}
return true;
}
分析函数pre_arm_ekf_attitude_check()
bool AP_Arming_Copter::pre_arm_ekf_attitude_check()
{
//获取EKF过滤器状态---- get ekf filter status
nav_filter_status filt_status = _inav.get_filter_status();
return filt_status.flags.attitude;
}7、电压解锁检查
bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
{
#if HAL_HAVE_BOARD_VOLTAGE
//检查电压------check board voltage
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE))
{
if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage");
}
return false;
}
}
#endif
Parameters &g = copter.g;
//检查电池电压----check battery voltage
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE))
{
if (copter.failsafe.battery)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Battery failsafe");
}
return false;
}
//如果USB连接，则跳过所有以下检查---all following checks are skipped if USB is connected
if (copter.ap.usb_connected)
{
return true;
}
//检查电池是否耗尽---check if battery is exhausted
if (copter.battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
}
return false;
}
//呼叫父类电池检查----call parent battery checks
if (!AP_Arming::battery_checks(display_failure))
{
return false;
}
}
return true;
}8、log解锁检查
bool AP_Arming::logging_checks(bool report)
{
if ((checks_to_perform & ARMING_CHECK_ALL) ||(checks_to_perform & ARMING_CHECK_LOGGING))
{
if (DataFlash_Class::instance()->logging_failed())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Logging failed");
}
return false;
}
if (!DataFlash_Class::instance()->CardInserted())
{
if (report)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: No SD card");
}
return false;
}
}
return true;
}9、参数解锁检查
bool AP_Arming_Copter::parameter_checks(bool display_failure)
{
//检查各种参数值------check various parameter values
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS))
{
//确保CH7和CH8具有不同的功能-------ensure ch7 and ch8 have different functions
if (copter.check_duplicate_auxsw())
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options");
}
return false;
}
//故障安全参数检查-----failsafe parameter checks
if (copter.g.failsafe_throttle)
{
// check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
//检查油门最小值是高于油门故障安全触发器值，触发器高于PPM编码器的信号损失值900。
if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE");
}
return false;
}
}
//倾斜角参数校核-------lean angle parameter check
if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX");
}
return false;
}
//动平衡参数检查-----acro balance parameter check
if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) {
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
}
return false;
}
#if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED
//检查测距仪，假如光流使能了--------check range finder if optflow enabled
if (copter.optflow.enabled() && !copter.rangefinder.pre_arm_check())
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
}
return false;
}
#endif
#if FRAME_CONFIG == HELI_FRAME
//直升机参数检查-----check helicopter parameters
if (!copter.motors->parameter_check(display_failure))
{
return false;
}
#endif // HELI_FRAME
//检查丢失的地形数据----check for missing terrain data
if (!pre_arm_terrain_check(display_failure))
{
return false;
}
//检查ADSB避免故障安全----check adsb avoidance failsafe
if (copter.failsafe.adsb)
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected");
}
return false;
}
//检查近距传感器-----check for something close to vehicle
if (!pre_arm_proximity_check(display_failure))
{
return false;
}
}
return true;
}10、电机解锁检查
bool AP_Arming_Copter::motor_checks(bool display_failure)
{
//检查电机是否正确初始化-------------check motors initialised correctly
if (!copter.motors->initialised_ok())
{
if (display_failure)
{
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check firmware or FRAME_CLASS");
}
return false;
}
return true;
}11、飞行油门解锁检查
bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)
{
// check throttle is above failsafe throttle
// this is near the bottom to allow other failures to be displayed before checking pilot throttle
//检查油门值是否高于故障油门值，接近最低油门值时，以允许在检查油门之前显示其他故障
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC))
{
if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value)
{
if (display_failure)
{
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe");
#endif
}
return false;
}
}
return true;
} 第四：Pre-Arm如何显示到地面站提示错误信息
1.gcs_send_text()函数
void AP_Arming_Copter::gcs_send_text(MAV_SEVERITY severity, const char *str)
{
copter.gcs_send_text(severity, str);
}void Copter::gcs_send_text(MAV_SEVERITY severity, const char *str)
{
gcs().send_statustext(severity, 0xFF, str);
notify.send_text(str);
}
1） gcs().send_statustext(severity, 0xFF, str)
void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text)
{
if (dataflash_p != nullptr)
{
dataflash_p->Log_Write_Message(text);
}
// add statustext message to FrSky lib queue
if (frsky_telemetry_p != NULL)
{
frsky_telemetry_p->queue_message(severity, text);
}
// filter destination ports to only allow active ports.
statustext_t statustext{};
statustext.bitmask = (GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask() ) & dest_bitmask;
if (!statustext.bitmask) {
// nowhere to send
return;
}
statustext.msg.severity = severity;
strncpy(statustext.msg.text, text, sizeof(statustext.msg.text));
// The force push will ensure comm links do not block other comm links forever if they fail.
// If we push to a full buffer then we overwrite the oldest entry, effectively removing the
// block but not until the buffer fills up.
_statustext_queue.push_force(statustext);
// try and send immediately if possible
service_statustext();
}2） notify.send_text(str)
void AP_Notify::send_text(const char *str)
{
strncpy(_send_text, str, sizeof(_send_text));
_send_text[sizeof(_send_text)-1] = 0;
_send_text_updated_millis = AP_HAL::millis();
}2.copter.gcs_send_text_fmt函数
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.send_statustext_all（）函数
void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...)
{
char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
va_list arg_list;
va_start(arg_list, fmt);
hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list);
va_end(arg_list);
text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = 0;
gcs().send_statustext(severity, mavlink_active | chan_is_streaming, text);
}