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└──> <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

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_actionAP_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└──> <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

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_actionAP_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_actionCopter::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├──> <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

3. 流程

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：直接上锁电机停转。

3.1 set_mode_land_with_pause

1. sets mode to LAND
2. triggers 4 second delay before descent starts
3. descent aircraft
4. land detected
5. 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 changeAP_Notify::events.failsafe_mode_change = 1;
}

3.2 set_mode_RTL_or_land_with_pause

1. sets mode to RTL if possible
2. 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 Landif (!set_mode(Mode::Number::RTL, reason)) {// set mode to land will trigger mode change notification to pilotset_mode_land_with_pause(reason);} else {// alert pilot to mode changeAP_Notify::events.failsafe_mode_change = 1;}
}

3.3 set_mode_SmartRTL_or_RTL

1. sets mode to SMART_RTL if possible
2. or RTL if possible
3. 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 landif (!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

1. sets mode to SMART_RTL if possible
2. 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 Landif (!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

1. 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 == ENABLEDif (set_mode(Mode::Number::AUTO_RTL, reason)) {AP_Notify::events.failsafe_mode_change = 1;return;}
#endifgcs().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

1. Sets mode to Brake
2. 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 == ENABLEDif (set_mode(Mode::Number::BRAKE, reason)) {AP_Notify::events.failsafe_mode_change = 1;return;}
#endifgcs().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飞控启动&运行过程简介