之前在ArduPilot飞控之FAILSAFE机制中，针对Ardupilot的FAILSAFE机制进行了相对完整的介绍。
本章节将从代码的角度来看FAILSAFE策略。
Copter::do_failsafe_action的入口实际在程序中非常多，因此，实际场景非常复杂。这里将从代码角度进行分门别类。
系统上电初始化时，对飞控RC信号以及ESC通信进行检查，设置对应的FAILSAFE状态。
Copter::init_ardupilot
└──> Copter::esc_calibration_startup_check
├──> [delay up to 2 second for first radio input]
│ └──> Copter::read_radio/Copter::set_throttle_and_failsafe
│ └──> Copter::set_failsafe_radio
│ └──> Copter::failsafe_radio_on_event
│ └──> Copter::do_failsafe_action
└──> <ESCCalibrationModes::ESCCAL_PASSTHROUGH_ALWAYS>
└──> Copter::esc_calibration_passthrough
└──> Copter::read_radio/Copter::set_throttle_and_failsafe
└──> Copter::set_failsafe_radio
└──> Copter::failsafe_radio_on_event
└──> Copter::do_failsafe_action
定时250Hz频率进行RC链路的轮询检查，如果出现链路通信问题，则触发FAILSAFE策略。
Copter::rc_loop // 250Hz task
└──> Copter::read_radio/Copter::set_throttle_and_failsafe
└──> Copter::set_failsafe_radio
└──> Copter::failsafe_radio_on_event
└──> Copter::do_failsafe_action
定时10Hz频率进行电池的轮询检查，如果出现电池阈值超限，则触发FAILSAFE策略。
Copter::update_batt_compass // 10Hz task
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
EKF设置HOME位置时，对电池进行阈值检查，如果出现电池阈值超限，则触发FAILSAFE策略。
Copter::update_home_from_EKF // FAST_TASK
└──> Copter::set_home_to_current_location
└──> Copter::mavlink_compassmot
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
GCS通信链路异常 或者出现DeadReckon异常，则触发FAILSAFE策略。
Copter::three_hz_loop // 3.3Hz task
├──> Copter::failsafe_gcs_check
│ └──> Copter::failsafe_gcs_on_event
│ └──> Copter::do_failsafe_action
└──> Copter::failsafe_deadreckon_check
└──> Copter::do_failsafe_action
通过MAVLink指令设置HOME位置/起飞前检查指令，检测电池阈值、油门阈值、RC链路，触发FAILSAFE策略。
GCS::update_receive // 400Hz task
└──> GCS_MAVLINK::update_receive
└──> GCS_MAVLINK_Copter::packetReceived
└──> GCS_MAVLINK_Copter::handleMessage
└──> GCS_MAVLINK::handle_common_message
├──> <> GCS_MAVLINK::handle_command_int
│ └──> GCS_MAVLINK_Copter::handle_command_int_packet
│ └──> GCS_MAVLINK::handle_command_int_packet
│ └──> GCS_MAVLINK::handle_command_int_do_set_home
│ └──> GCS_MAVLINK_Copter::set_home_to_current_location
│ └──> Copter::set_home_to_current_location
│ └──> Copter::mavlink_compassmot
│ └──> AP_BattMonitor::read
│ └──> AP_BattMonitor::check_failsafes
│ └──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
│ └──> Copter::do_failsafe_action
└──> <> GCS_MAVLINK::handle_command_long
├──> GCS_MAVLINK_Copter::handle_command_long_packet
│ └──> GCS_MAVLINK::handle_command_long_packet
│ ├──> <> GCS_MAVLINK::handle_command_do_set_home
│ │ └──> GCS_MAVLINK_Copter::set_home_to_current_location
│ │ └──> Copter::set_home_to_current_location
│ │ └──> Copter::mavlink_compassmot
│ │ └──> AP_BattMonitor::read
│ │ └──> AP_BattMonitor::check_failsafes
│ │ └──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
│ │ └──> Copter::do_failsafe_action
│ └──> <> GCS_MAVLINK::handle_command_component_arm_disarm
│ └──> AP_Arming_Copter::arm
│ └──> Copter::set_home_to_current_location
│ └──> Copter::mavlink_compassmot
│ └──> AP_BattMonitor::read
│ └──> AP_BattMonitor::check_failsafes
│ └──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
│ └──> Copter::do_failsafe_action
└──> <> GCS_MAVLINK::handle_command_preflight_calibration
└──> GCS_MAVLINK::_handle_command_preflight_calibration
└──> GCS_MAVLINK_Copter::_handle_command_preflight_calibration
└──> Copter::mavlink_compassmot
├──> <> AP_BattMonitor::read
│ └──> AP_BattMonitor::check_failsafes
│ └──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
│ └──> Copter::do_failsafe_action
└──> <> Copter::read_radio/Copter::set_throttle_and_failsafe
└──> Copter::set_failsafe_radio
└──> Copter::failsafe_radio_on_event
└──> Copter::do_failsafe_action
AP_Vehicle::setup
└──> GCS::setup_console
└──> GCS::create_gcs_mavlink_backend
└──> GCS::new_gcs_mavlink_backend
└──> GCS_Copter::new_gcs_mavlink_backend
└──> GCS_MAVLINK_Copter
注：MAVLink链路在setup时建立。
强制手动解锁电机前，检查电池阈值，超限则触发FAILSAFE策略。
Copter::arm_motors_check // 10Hz task
└──> <yaw right for ARM_DELAY> AP_Arming_Copter::arm
└──> Copter::set_home_to_current_location
└──> Copter::mavlink_compassmot
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
自动导航任务执行时，每次执行任务命令时，对电池阈值进行检测，超限则触发FAILSAFE策略。
AP_Mission::resume
└──> AP_Mission::start_command
└──> ModeAuto::start_command/_cmd_start_fn
└──> ModeAuto::do_set_home
└──> Copter::set_home_to_current_location
└──> Copter::mavlink_compassmot
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
AP_Mission::start/AP_Mission::update/AP_Mission::set_current_cmd
└──> AP_Mission::advance_current_nav_cmd
└──> AP_Mission::start_command
└──> ModeAuto::start_command/_cmd_start_fn
└──> ModeAuto::do_set_home
└──> Copter::set_home_to_current_location
└──> Copter::mavlink_compassmot
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
Copter::update_flight_mode // FAST_TASK
└──> ModeAuto::run
└──> AP_Mission::update
└──> AP_Mission::advance_current_do_cmd
└──> AP_Mission::start_command
└──> ModeAuto::start_command/_cmd_start_fn
└──> ModeAuto::do_set_home
└──> Copter::set_home_to_current_location
└──> Copter::mavlink_compassmot
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
注：关于AP_Mission将在另一个章节再做描述。
只有使能TOY_MODE_ENABLED时，ToyMode才会被被启用。这里仅仅提供一个入口位置。
ToyMode::update // TOY_MODE_ENABLED 10Hz
├──> <ACTION_ARM_LAND_RTL> ToyMode::action_arm
│ └──> AP_Arming_Copter::arm
│ └──> Copter::set_home_to_current_location
│ └──> Copter::mavlink_compassmot
│ └──> AP_BattMonitor::read
│ └──> AP_BattMonitor::check_failsafes
│ └──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
│ └──> Copter::do_failsafe_action
│
│
│
│
└──> <ENABLE_LOAD_TEST> AP_Arming_Copter::arm
└──> Copter::set_home_to_current_location
└──> Copter::mavlink_compassmot
└──> AP_BattMonitor::read
└──> AP_BattMonitor::check_failsafes
└──> Copter::handle_battery_failsafe/_battery_failsafe_handler_fn
└──> Copter::do_failsafe_action
do_failsafe_action被触发后，其内部流程主要有以下几个：
Copter::do_failsafe_action
├──> 【1】<FailsafeAction::NONE> return
├──> 【2】<FailsafeAction::LAND> set_mode_land_with_pause
├──> 【3】<FailsafeAction::RTL> set_mode_RTL_or_land_with_pause
├──> 【4】<FailsafeAction::SMARTRTL> set_mode_SmartRTL_or_RTL
├──> 【5】<FailsafeAction::SMARTRTL_LAND> set_mode_SmartRTL_or_land_with_pause
├──> 【6】<FailsafeAction::TERMINATE>
│ ├──> <ADVANCED_FAILSAFE == ENABLED> g2.afs.gcs_terminate(true, "Failsafe")
│ └──> <else> arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE)
├──> 【7】<FailsafeAction::AUTO_DO_LAND_START> set_mode_auto_do_land_start_or_RTL
├──> 【8】<FailsafeAction::BRAKE_LAND> set_mode_brake_or_land_with_pause
└──> 【9】<AP_GRIPPER_ENABLED> <failsafe_option(FailsafeOption::RELEASE_GRIPPER)>
└──> copter.g2.gripper.release()
enum class FailsafeAction : uint8_t {
NONE = 0,
LAND = 1,
RTL = 2,
SMARTRTL = 3,
SMARTRTL_LAND = 4,
TERMINATE = 5,
AUTO_DO_LAND_START = 6,
BRAKE_LAND = 7
};
注：NONE：表示不作出任何响应； TERMINATE：直接上锁电机停转。
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_land_with_pause(ModeReason reason)
{
set_mode(Mode::Number::LAND, reason);
mode_land.set_land_pause(true);
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason)
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(Mode::Number::RTL, reason)) {
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause(reason);
} else {
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
}
// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
{
// attempt to switch to SmartRTL, if this failed then attempt to RTL
// if that fails, then land
if (!set_mode(Mode::Number::SMART_RTL, reason)) {
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
set_mode_RTL_or_land_with_pause(reason);
} else {
AP_Notify::events.failsafe_mode_change = 1;
}
}
// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
{
// attempt to switch to SMART_RTL, if this failed then switch to Land
if (!set_mode(Mode::Number::SMART_RTL, reason)) {
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
set_mode_land_with_pause(reason);
} else {
AP_Notify::events.failsafe_mode_change = 1;
}
}
This can come from failsafe or RC option
// Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param
// This can come from failsafe or RC option
void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
{
#if MODE_AUTO_ENABLED == ENABLED
if (set_mode(Mode::Number::AUTO_RTL, reason)) {
AP_Notify::events.failsafe_mode_change = 1;
return;
}
#endif
gcs().send_text(MAV_SEVERITY_WARNING, "Trying RTL Mode");
set_mode_RTL_or_land_with_pause(reason);
}
// Sets mode to Brake or LAND with 4 second delay before descent starts
// This can come from failsafe or RC option
void Copter::set_mode_brake_or_land_with_pause(ModeReason reason)
{
#if MODE_BRAKE_ENABLED == ENABLED
if (set_mode(Mode::Number::BRAKE, reason)) {
AP_Notify::events.failsafe_mode_change = 1;
return;
}
#endif
gcs().send_text(MAV_SEVERITY_WARNING, "Trying Land Mode");
set_mode_land_with_pause(reason);
}
do_failsafe_action大体上有以下代码触发入口，以及对应的应对策略。
FAILSAFE策略【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot飞控之FAILSAFE机制
【3】ArduPilot之开源代码Task介绍
【4】ArduPilot飞控启动&运行过程简介
EKF异常处理机制稍微特殊一些，并没有采用do_failsafe_action这个例程，而是专门有一套策略，详见：下面ekf_check任务。
Copter::ekf_check // 10Hz task
└──> Copter::failsafe_ekf_event
在ModeAuto执行mission的时候，如果涉及到需要GPS时，会触发一次检查：
ModeAuto::set_submode
└──> Copter::failsafe_ekf_recheck
└──> Copter::failsafe_ekf_event
// re-check if the flight mode requires GPS but EKF failsafe is active
// this should be called by flight modes that are changing their submode from one that does NOT require a position estimate to one that does
void Copter::failsafe_ekf_recheck()
{
// return immediately if not in ekf failsafe
if (!failsafe.ekf) {
return;
}
// trigger EKF failsafe action
failsafe_ekf_event();
}
EKF检查例程主要涉及以下措施，详见：
mode_land.do_not_use_GPS()set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)set_mode_land_with_pause(ModeReason::EKF_FAILSAFE)// failsafe_ekf_event - perform ekf failsafe
void Copter::failsafe_ekf_event()
{
// EKF failsafe event has occurred
failsafe.ekf = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
// if disarmed take no action
if (!motors->armed()) {
return;
}
// sometimes LAND *does* require GPS so ensure we are in non-GPS land
if (flightmode->mode_number() == Mode::Number::LAND && landing_with_GPS()) {
mode_land.do_not_use_GPS();
return;
}
// does this mode require position?
if (!copter.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
return;
}
// take action based on fs_ekf_action parameter
switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD:
// AltHold
if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
}
break;
case FS_EKF_ACTION_LAND:
case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
default:
set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
break;
}
// set true if ekf action is triggered
AP_Notify::flags.failsafe_ekf = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe: changed to %s Mode", flightmode->name());
}
文章浏览阅读1.5k次。StringTokenizer類的nextToken()方法用於從此StringTokenizer依次返回下一個標記。用法:public String nextToken()參數：該方法不帶任何參數。返回值：該方法返回字符串令牌化程序行中存在的下一個令牌。下麵的程序說明StringTokenizer的nextToken()方法的用法：示例1:// Java code to illustrate n..._next token的用法
文章浏览阅读268次。网上一些校正系统时间的方法，大多需要手动操作，有没有什么工具可以自动完成相关的操作呢？ 可以试试NetTime工具（http://www.timesynctool.com/）.安装该工具时会创建一个自启动服务，以后每次启动系统，它都会随之启动并自动校正系统时间。_timesync 自动
文章浏览阅读2.3k次。即使我们使用了类似 *{margin: 0;padding: 0;} 这样的代码重置了浏览器默认样式,也会发现类似标签这种inline-block元素，它们之间也还存在着间距。demo:默认情况123456789101112131415_css处理手机自动留白安全距离
文章浏览阅读2.0k次，点赞14次，收藏17次。第一章、概述1.1.1数据分析：采用适当的统计分析方法对收集到的数据进行分析、概括和总结，对数据进行恰当的描述，提取出有用的信息的过程。1.1.2数据挖掘：从海量数据种通过相关的算法来发现隐藏在数据中的规律和知识的过程。1.1.3知识发现的过程1.1.4数据分析与数据挖掘的区别1.1.5数据分析与数据挖掘的联系数据-------数据分析----->信息-------数据挖掘-------->知识1.2分析与挖掘的数据类型1.3数据分析..._数据分析与数据挖掘
文章浏览阅读312次。Python实现WOA智能鲸鱼优化算法优化XGBoost分类模型(XGBClassifier算法)项目实战_鲸鱼算法 xgboost
文章浏览阅读3.9k次。android.webkit.WebView/WebViewClient/WebChromeClient使用android.webkit.WebView控件在xml布局文件中定义 android:id=”@+id/webkit01” android:layout:width=”fill_parent” android:layout:height=”fill_par_android.webkit.webview
文章浏览阅读5.4k次。2)完全二叉树中如果每颗子树的最大值都在顶部就是大根堆3)完全二叉树中如果每颗子树的最小值都在顶部就是小根堆4)堆结构的向上调整和向下调整算法向上调整向下调整5)堆结构某个元素的增大和减少6)优先级队列结构就是堆结构空树是完全二叉树,完全二叉树的性质:如果父亲下标为i : 左孩子:2*i+1 右孩子: 2*i+2如果孩子下标为i: 父亲的下标:(i-1)/2。_堆排序例题
文章浏览阅读2.8k次。HotSpot VM_hotspot虚拟机
文章浏览阅读1.1k次。导读：之前发布了云平台技术栈(ps:点击可查看)，本文主要说一下其中的容器调度技术！作者：阿里中间件，公众号：云栖社区本文以资源分配理念：拍卖、预算、抢占出发，引出Bor..._资源调度编排种类
文章浏览阅读2w次，点赞43次，收藏94次。Matplotlib解决多子图时的间距问题Create SubplotsAdjust Spacing of Subplots Using tight_layout()Adjust Spacing of Subplot TitlesAdjust Spacing of Overall Title参考链接Often you may use subplots to display multiple plots alongside each other in Matplotlib. Unfortunately, t_matplotlib 子图间隔
文章浏览阅读1.8w次，点赞14次，收藏50次。最近在使用msys2的时候，无法使用pacman -Syu进行更新，会出现如下提示：原因是密钥无法信赖(rely on)msys2-keyring密钥服务器。msys2官方提供了以下方法：# pacman-key --verify msys2-keyring-r21.b39fb11-1-any.pkg.tar.xz{.sig,}解决办法：１． 下载 msys2-keyring-r21.b39fb11-1-any.pkg.tar.xz 软件包# curl -O http://repo.msys_由 msys2-keyring-r21.b39fb11-1-any.pkg.tar.xz.sig 认定的签名无法验证
文章浏览阅读647次。的博客地址：https://juejin.cn/user/3597257779197021介绍 WindowManagerService 简称 WMS ，是系统的核心服务，主要分为四大部分，分别是窗口管理，窗口动画，输入系统中转站和Surface 管理 。WMS 的职责很多，主要的就是下面这几点：窗口管理：WMS是窗口的管理者，负责窗口的启动，添加和删除，另外窗口的大小也时有 WMS 管理的，管理..._android windowmanagerpolicy详解