ArduPilot开源飞控之do_failsafe_action
ArduPilot开源飞控之do_failsafe_action
- 1. 源由
- 2. 触发
-
- 2.1 初始化RC链路检测
- 2.2 定时RC链路检测
- 2.3 电池阈值检测
- 2.4 HOME位置设置异常
- 2.5 GCS链路 & DeadReckon检查
- 2.6 MAVLink指令 (GCS/CompanionComputer)
- 2.7 强制手动解锁电池检查
- 2.8 自动导航任务电池检查
- 2.9 ToyMode
- 3. 流程
-
- 3.1 set_mode_land_with_pause
- 3.2 set_mode_RTL_or_land_with_pause
- 3.3 set_mode_SmartRTL_or_RTL
- 3.4 set_mode_SmartRTL_or_land_with_pause
- 3.5 set_mode_auto_do_land_start_or_RTL
- 3.6 set_mode_brake_or_land_with_pause
- 4. 总结
-
- 4.1 代码触发入口
- 4.2 `FAILSAFE`策略
- 5. 参考资料
- 6. 补充资料 - EKF failsafe
1. 源由
之前在ArduPilot飞控之FAILSAFE机制中，针对Ardupilot的FAILSAFE机制进行了相对完整的介绍。
本章节将从代码的角度来看FAILSAFE策略。
2. 触发
Copter::do_failsafe_action的入口实际在程序中非常多，因此，实际场景非常复杂。这里将从代码角度进行分门别类。
2.1 初始化RC链路检测
系统上电初始化时，对飞控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
└──>
└──> 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
2.2 定时RC链路检测
定时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
2.3 电池阈值检测
定时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
2.4 HOME位置设置异常
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
2.5 GCS链路 & DeadReckon检查
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
2.6 MAVLink指令 (GCS/CompanionComputer)
通过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时建立。
2.7 强制手动解锁电池检查
强制手动解锁电机前，检查电池阈值，超限则触发FAILSAFE策略。
Copter::arm_motors_check // 10Hz task
└──> 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
2.8 自动导航任务电池检查
自动导航任务执行时，每次执行任务命令时，对电池阈值进行检测，超限则触发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将在另一个章节再做描述。
2.9 ToyMode
只有使能TOY_MODE_ENABLED时，ToyMode才会被被启用。这里仅仅提供一个入口位置。
ToyMode::update // TOY_MODE_ENABLED 10Hz
├──> 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
│
│
│
│
└──> 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
3. 流程
do_failsafe_action被触发后，其内部流程主要有以下几个：
Copter::do_failsafe_action
├──> 【1】 return
├──> 【2】 set_mode_land_with_pause
├──> 【3】 set_mode_RTL_or_land_with_pause
├──> 【4】 set_mode_SmartRTL_or_RTL
├──> 【5】 set_mode_SmartRTL_or_land_with_pause
├──> 【6】
│ ├──> g2.afs.gcs_terminate(true, "Failsafe")
│ └──> arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE)
├──> 【7】 set_mode_auto_do_land_start_or_RTL
├──> 【8】 set_mode_brake_or_land_with_pause
└──> 【9】
└──> 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：直接上锁电机停转。
3.1 set_mode_land_with_pause
- sets mode to LAND
- triggers 4 second delay before descent starts
- descent aircraft
- land detected
- disarm
// 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;
}
3.2 set_mode_RTL_or_land_with_pause
- sets mode to RTL if possible
- or LAND triggers 4 second delay before descent starts
// 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;
}
}
3.3 set_mode_SmartRTL_or_RTL
- sets mode to SMART_RTL if possible
- or RTL if possible
- or LANDtriggers 4 second delay before descent starts
// 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;
}
}
3.4 set_mode_SmartRTL_or_land_with_pause
- sets mode to SMART_RTL if possible
- or LAND with 4 second delay before descent starts
// 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;
}
}
3.5 set_mode_auto_do_land_start_or_RTL
- Sets mode to Auto and jumps to DO_LAND_START
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);
}
3.6 set_mode_brake_or_land_with_pause
- Sets mode to Brake
- or LAND with 4 second delay before descent starts
// 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);
}
4. 总结
do_failsafe_action大体上有以下代码触发入口，以及对应的应对策略。
4.1 代码触发入口
- 1 初始化RC链路检测
- 2 定时RC链路检测
- 3 电池阈值检测
- 4 HOME位置设置异常
- 5 GCS链路 & DeadReckon检查
- 6 MAVLink指令 (GCS/CompanionComputer)
- 7 强制手动解锁电池检查
- 8 自动导航任务电池检查
- 9 ToyMode
4.2 FAILSAFE策略
- 1 NONE
- 2 LAND
- 3 RTL
- 4 SMARTRTL
- 5 SMARTRTL_LAND
- 6 TERMINATE
- 7 AUTO_DO_LAND_START
- 8 BRAKE_LAND
- 9 GRIPPER_RELEASE
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot飞控之FAILSAFE机制
【3】ArduPilot之开源代码Task介绍
【4】ArduPilot飞控启动&运行过程简介
6. 补充资料 - EKF failsafe
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());
}