ardupilot/Ardupilot V4.3.4 源码解析：mode.cpp

#include "Plane.h"

Mode::Mode()

#if HAL_QUADPLANE_ENABLED

    : quadplane(plane.quadplane),

    pos_control(plane.quadplane.pos_control),

    attitude_control(plane.quadplane.attitude_control),

    loiter_nav(plane.quadplane.loiter_nav),

    poscontrol(plane.quadplane.poscontrol)

#endif

{

}

void Mode::exit()

{

    // 调用子类的退出函数

    _exit();

}

bool Mode::enter()

{

#if AP_SCRIPTING_ENABLED

    // 重置nav_scripting.enabled

    plane.nav_scripting.enabled = false;

#endif

    //  取消倒飞状态

    plane.auto_state.inverted_flight = false;

    /*在开始任务时，不要跨越航迹。

    这句话的意思是在启动任务时要确保飞行或航行的路径与预定的航迹一致，不要偏离原定航线。

    这是为了确保任务的顺利执行和安全性。*/

    plane.auto_state.next_wp_crosstrack = false;

    // 重置自动着陆检测

    plane.auto_state.checked_for_autoland = false;

    // 零定向锁定误差

    plane.steer_state.locked_course_err = 0;

    plane.steer_state.locked_course = false;

    // 重置碰撞检测

    plane.crash_state.is_crashed = false;

    plane.crash_state.impact_detected = false;

    // 重置外部姿态引导

    plane.guided_state.last_forced_rpy_ms.zero();

    plane.guided_state.last_forced_throttle_ms = 0;

#if OFFBOARD_GUIDED == ENABLED

    plane.guided_state.target_heading = -4; // 弧度在-3.14至3.14范围内，因此默认值需要超出该范围

    plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;

    plane.guided_state.target_airspeed_cm = -1; // 同上，尽管空速-1在飞机上很少见

    plane.guided_state.target_alt = -1; // 同上，尽管目标高度-1在飞机上很少见

    plane.guided_state.last_target_alt = 0;

#endif

#if CAMERA == ENABLED

    plane.camera.set_is_auto_mode(this == &plane.mode_auto);

#endif

    // 零初始俯仰角和最大空速

    plane.auto_state.highest_airspeed = 0;

    plane.auto_state.initial_pitch_cd = plane.ahrs.pitch_sensor;

    // 在模式更改时禁用尾轮起飞模式

    plane.auto_state.fbwa_tdrag_takeoff_mode = false;

    // 标记新的任务起始位置为当前位置

    plane.prev_WP_loc = plane.current_loc;

 

    // 新模式意味着新的盘旋

    plane.loiter.start_time_ms = 0;

    // 记录模式更改时间

    plane.last_mode_change_ms = AP_HAL::millis();

    // 设置VTOL自动模式状态

    plane.auto_state.vtol_mode = is_vtol_mode();

    plane.auto_state.vtol_loiter = false;

    // 初始化用于AUTO和GUIDED模式下的DO_CHANGE_SPEED命令中的速度变量。

    plane.new_airspeed_cm = -1;

#if HAL_QUADPLANE_ENABLED

    quadplane.mode_enter();

#endif

    bool enter_result = _enter();

    if (enter_result) {

        // -------------------

        // 这些必须在AFTER_enter()之后完成，因为它们使用结果设置更多的标志

        // 在auto_throttle模式下开始时抑制油门

        plane.throttle_suppressed = does_auto_throttle();

#if HAL_ADSB_ENABLED

        plane.adsb.set_is_auto_mode(does_auto_navigation());

#endif

        // 在模式更改时重置航向积分器

        plane.steerController.reset_I();

        // update RC failsafe, as mode change may have necessitated changing the failsafe throttle

        // 更新遥控器故障保护（failsafe），因为模式更改可能需要更改失控保护油门

        /*为了根据模式的变化调整失控保护油门，可以更新RC失控保护的设置。具体步骤如下：

          监测飞行模式的变化。检测到模式变化后，获取新的飞行模式。

          根据新的飞行模式，更新失控保护油门的设置。根据你的飞行控制系统和相应的API或命令进行设置调整。

          下面是一个示例，展示了如何在ArduPilot飞控中更新RC失控保护的阈值：

          def update_rc_failsafe_throttle(new_mode):

             if new_mode == "AUTO":

                set_rc_failsafe_throttle(1100)  # 设置失控保护油门为1100

             elif new_mode == "GUIDED":

                set_rc_failsafe_throttle(1200)  # 设置失控保护油门为1200

             elif new_mode == "RTL":

             set_rc_failsafe_throttle(1300)  # 设置失控保护油门为1300

               else:

              set_rc_failsafe_throttle(1000)  # 设置默认失控保护油门为1000

             # 监测飞行模式的变化

        if mode_changed:

             new_mode = get_new_mode()  # 获取新的飞行模式

             update_rc_failsafe_throttle(new_mode)  # 更新RC失控保护油门设置

           请根据自己的飞控系统和编程语言进行适当的调整和修改。确保根据系统的API和命令来设置RC失控保护油门，以便在模式变化时达到预期的效果。

*/

        plane.control_failsafe();

#if AP_FENCE_ENABLED

        // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)

        // but it should be harmless to disable the fence temporarily in these situations as well

        // 当飞行员在电子围栏范围内进行飞行模式变更请求时，表明飞行员试图手动恢复控制。

        // 这种飞行模式转换可能是自动的（如围栏、电池、GPS或遥控器失效保护）

        // 但是在这些情况下将围栏短暂禁用也应该是无害的

        /*如果飞行模式变更是由自动的失控保护触发的（例如，篱笆、电池、GPS或地面控制站（GCS）的失控保护），

        可以根据相应的失控保护条件来处理这种情况。

        以下是一个示例代码片段，展示了如何处理自动失控保护触发的飞行模式变更请求：

        def handle_automatic_flight_mode_change(trigger):

    if trigger == "FENCE":

        recover_control_from_fence_breach()  # 从篱笆违规中恢复控制

    elif trigger == "BATTERY":

        return_home_on_low_battery()  # 当电池电量低时自动返航

    elif trigger == "GPS":

        switch_to_altitude_hold_mode()  # 当GPS信号丢失时切换到定高模式

    elif trigger == "GCS":

        land_safely_on_lost_connection()  # 当与地面控制站失去连接时安全降落

    else:

        reject_mode_change_request()  # 拒绝其他失控保护触发的变更请求

    # 监测自动失控保护触发的飞行模式变更请求

    trigger = get_auto_fail-safe_trigger()  # 获取触发失控保护的条件

    handle_automatic_flight_mode_change(trigger)  # 处理自动失控保护触发的飞行模式变更请求

    在这个例子中，handle_automatic_flight_mode_change() 函数根据失控保护触发的条件来执行相应的操作。

    可以根据具体的失控保护条件来自定义适当的动作，例如恢复控制、返航、切换模式或安全降落。

        */

        plane.fence.manual_recovery_start();

#endif

    }

    return enter_result;

}

bool Mode::is_vtol_man_throttle() const

{

#if HAL_QUADPLANE_ENABLED

    if (plane.quadplane.tailsitter.is_in_fw_flight() &&

        plane.quadplane.assisted_flight) {

        // 正在完全过渡到垂起辅助增稳前向飞行的可选倾转机翼（tailsitter）。

        // 在这种情况下，前推油门直接驱动垂直油门，所以需要设置垂直油门状态以匹配前推油门状态。

        // 然而，布尔变量的取值与实际意义相反，前推油门使用'does_auto_throttle'，而垂直油门使用'is_vtol_man_throttle'。

        //请确保正确处理这些变量，以保持飞行器所需的行为。

        return !does_auto_throttle();

    }

#endif

    return false;

}