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

  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 change
    AP_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 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

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

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

  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 == 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

  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 == 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());
}

你可能感兴趣的:(ArduPilot,开源)