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

于 2023-08-07 08:30:00 发布
阅读量204 收藏 1
点赞数 1
分类专栏： ardupilot 固定翼/垂起代码分析-4.2.3版本 Ardupilot V4.3.4 源码解析 文章标签： 开源 开源软件
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_17031687/article/details/132123779
版权

/*

    对飞机进行解锁前检查

 */

#include "AP_Arming.h"

#include "Plane.h"

#include "qautotune.h"

constexpr uint32_t AP_ARMING_DELAY_MS = 2000;

                                              // 从解锁到马达启动的延迟

const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {

    // 来自上位机的变量

    AP_NESTEDGROUPINFO(AP_Arming, 0),

    // index 3 was RUDDER and should not be used

    AP_GROUPEND

};

//  如果需要地形数据库加载所有数据，则预期返回true。

bool AP_Arming_Plane::terrain_database_required() const

{

#if AP_TERRAIN_AVAILABLE

    if (plane.g.terrain_follow) {

        return true;

    }

#endif

    return AP_Arming::terrain_database_required();

}

/*

    对于飞机，还需要进行额外的解锁检查。

 */

bool AP_Arming_Plane::pre_arm_checks(bool display_failure)

{

    if (armed || require == (uint8_t)Required::NO) {

        //  如果我们已经处于解锁状态或者不需要进行解锁检查，则跳过检查。

        return true;

    }

    //解锁检查是否已禁用

    if (checks_to_perform == 0) {

        return true;

    }

    if (hal.util->was_watchdog_armed()) {

        /*在看门狗复位期间，可以绕过解锁检查，以允许在复位之前已被解锁的情况下进行飞行中的解锁。

        这样做是为了允许在BVLOS（超视距飞行）期间发生复位时返回起飞点，如果操作员可以通过遥测命令来解锁。

        通过绕过解锁检查，在复位之前已解锁的情况下允许飞机在飞行中继续解锁，

        可以为操作员提供一定的灵活性和可能性，使其能够回到起飞点或采取其他必要的措施，

        而无需进行普通的解锁检查。

        需要注意的是，这需要操作员能够通过遥测命令进行解锁，

        并且操作员必须具备足够的判断力，确保飞行条件安全，

        并在必要时执行适当的措施来保护人员和设备的安全。

        这种操作应该只在合理且安全的情况下进行，并且应遵守适用的法规和规定。

        */

        return true;

    }

    // 调用父类检查

    bool ret = AP_Arming::pre_arm_checks(display_failure);

    // 检查空速传感器

    ret &= AP_Arming::airspeed_checks(display_failure);

    if (plane.g.fs_timeout_long < plane.g.fs_timeout_short && plane.g.fs_action_short != FS_ACTION_SHORT_DISABLED) {

        check_failed(display_failure, "FS_LONG_TIMEOUT < FS_SHORT_TIMEOUT");

        ret = false;

    }

    if (plane.aparm.roll_limit_cd < 300) {

        check_failed(display_failure, "LIM_ROLL_CD too small (%u)", (unsigned)plane.aparm.roll_limit_cd);

        ret = false;

    }

    if (plane.aparm.pitch_limit_max_cd < 300) {

        check_failed(display_failure, "LIM_PITCH_MAX too small (%u)", (unsigned)plane.aparm.pitch_limit_max_cd);

        ret = false;

    }

    if (plane.aparm.pitch_limit_min_cd > -300) {

        check_failed(display_failure, "LIM_PITCH_MIN too large (%u)", (unsigned)plane.aparm.pitch_limit_min_cd);

        ret = false;

    }

    if (plane.aparm.airspeed_min < MIN_AIRSPEED_MIN) {

        check_failed(display_failure, "ARSPD_FBW_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN);

        ret = false;

    }

    if (plane.channel_throttle->get_reverse() &&

        Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled &&

        plane.g.throttle_fs_value <

        plane.channel_throttle->get_radio_max()) {

        check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle");

        ret = false;

    }

#if HAL_QUADPLANE_ENABLED

    ret &= quadplane_checks(display_failure);

#endif

    if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {

        check_failed(display_failure, "No mission loaded");

        ret = false;

    }

    // check adsb avoidance failsafe 检查ADS-B避障故障保护

    if (plane.failsafe.adsb) {

        check_failed(display_failure, "ADSB threat detected");

        ret = false;

    }

    if (SRV_Channels::get_emergency_stop()) {

        check_failed(display_failure,"Motors Emergency Stopped");

        ret = false;

    }

    if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){

       int16_t trim = plane.channel_throttle->get_radio_trim();

       if (trim < 1250 || trim > 1750) {

           check_failed(display_failure, "Throttle trim not near center stick(%u)",trim );

           ret = false;

       }

    }

    if (plane.mission.get_in_landing_sequence_flag() &&

        !plane.mission.starts_with_takeoff_cmd()) {

        check_failed(display_failure,"In landing sequence");

        ret = false;

    }

   

    return ret;

}

#if HAL_QUADPLANE_ENABLED

bool AP_Arming_Plane::quadplane_checks(bool display_failure)

{

    if (!plane.quadplane.enabled()) {

        return true;

    }

    if (!plane.quadplane.available()) {

        check_failed(display_failure, "Quadplane enabled but not running");

        return false;

    }

    bool ret = true;

    if (plane.scheduler.get_loop_rate_hz() < 100) {

        check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");

        ret = false;

    }

    char failure_msg[50] {};

    if (!plane.quadplane.motors->arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) {

        check_failed(display_failure, "Motors: %s", failure_msg);

        ret = false;

    }

    // 倾斜角度参数检查

    if (plane.quadplane.aparm.angle_max < 1000 || plane.quadplane.aparm.angle_max > 8000) {

        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check Q_ANGLE_MAX");

        ret = false;

    }

    if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.tiltrotor.enable > 0)) {

        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set TAILSIT_ENABLE 0 or TILT_ENABLE 0");

        ret = false;

    } else {

        if ((plane.quadplane.tailsitter.enable > 0) && !plane.quadplane.tailsitter.enabled()) {

            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tailsitter setup not complete, reboot");

            ret = false;

        }

        if ((plane.quadplane.tiltrotor.enable > 0) && !plane.quadplane.tiltrotor.enabled()) {

            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tiltrotor setup not complete, reboot");

            ret = false;

        }

    }

    // 确保控制器同意我们进行解锁操作是至关重要的。

    if (!plane.quadplane.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {

        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);

        ret = false;

    }

    if (!plane.quadplane.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {

        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);

        ret = false;

    }

    if (!plane.quadplane.motors->check_mot_pwm_params()) {

        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check Q_M_PWM_MIN/MAX");

        ret = false;

    }

    if (plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) {

        if (!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) {

            check_failed(display_failure,"not in Q mode");

            ret = false;

        }

        if ((plane.control_mode == &plane.mode_auto) && !plane.quadplane.is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {

            check_failed(display_failure,"not in VTOL takeoff");

            ret = false;

        }

    }

    if ((plane.control_mode == &plane.mode_auto) && !plane.mission.starts_with_takeoff_cmd()) {

        check_failed(display_failure,"missing takeoff waypoint");

        ret = false;

    }

    if (plane.control_mode == &plane.mode_rtl) {

        check_failed(display_failure,"in RTL mode");

        ret = false;

    }

   

    /*

     对于除了尾座式飞行器之外的所有四旋翼混合机型，启用Q_ASSIST_SPEED确实是一个很好的选择。

     Q_ASSIST_SPEED是基于固定翼模式下的辅助速度参数，用于提供在过渡阶段（从垂直起飞到水平飞行）中的辅助控制。

     通过启用Q_ASSIST_SPEED，可以在四旋翼与固定翼飞行的过渡中提供额外的控制和稳定性，从而改善飞行性能和操纵性。

     然而，对于尾座式飞行器来说，由于它们的设计和特性使得其在垂直起降和水平飞行之间的过渡相对较为简单，

     Q_ASSIST_SPEED可能不是必需的。尾座式飞行器通常具有更直接的控制和过渡特性，

     因此在这种情况下，禁用Q_ASSIST_SPEED可能更合适。

     需要注意的是，具体的飞行控制参数需要根据飞行器的设计和性能进行调整和优化。

     因此，在启用或禁用Q_ASSIST_SPEED之前，应仔细评估和测试飞行器的表现，并根据实际情况进行相应的参数设置。

     */

    if (check_enabled(ARMING_CHECK_PARAMETERS) &&

        is_zero(plane.quadplane.assist_speed) &&

        !plane.quadplane.tailsitter.enabled()) {

        check_failed(display_failure,"Q_ASSIST_SPEED is not set");

        ret = false;

    }

    return ret;

}

#endif // HAL_QUADPLANE_ENABLED

bool AP_Arming_Plane::ins_checks(bool display_failure)

{

    // 调用父类检查。

    if (!AP_Arming::ins_checks(display_failure)) {

        return false;

    }

    //  额外的针对飞机的特定检查

    if ((checks_to_perform & ARMING_CHECK_ALL) ||

        (checks_to_perform & ARMING_CHECK_INS)) {

        char failure_msg[50] = {};

        if (!AP::ahrs().pre_arm_check(true, failure_msg, sizeof(failure_msg))) {

            check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);

            return false;

        }

    }

    return true;

}

bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)

{

    if (method == AP_Arming::Method::RUDDER) {

        const AP_Arming::RudderArming arming_rudder = get_rudder_arming_type();

        if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {

            /*如果我们在这里发送一条消息来阻止舵机的解锁/禁用，那么进行舵面检查的人可能会因为接收到该消息而感到困扰。

            为了避免这种情况，我们可以采取一些措施来改善用户体验：

            1)隐藏或抑制消息：可以将消息设置为只在调试模式下显示，而不会在正常操作期间干扰用户。

            这样，只有在需要进行故障排除或验证时，用户才会看到相关消息。

            2)使用警告或通知级别消息：将该消息分类为警告或通知级别，这样用户就能够区分它们与更紧急的错误或故障消息。

            3) 提供详细信息和解决方案：在消息中提供更多的上下文信息和解决方案，

            以帮助用户了解为什么发生了此类消息以及如何解决。这将帮助他们在进行舵面检查时更好地理解所遇到的情况。*/

            return false;

        }

        // 如果油门没有关闭，飞行员就不能启动/禁用解锁。

        if (!is_zero(plane.get_throttle_input())){

            check_failed(true, "Non-zero throttle");

            return false;

        }

    }

    if (!plane.control_mode->allows_arming()) {

        check_failed(true, "Mode does not allow arming");

        return false;

    }

    // 是否已禁用启动检查？

    if (checks_to_perform == 0) {

        return true;

    }

    if (hal.util->was_watchdog_armed()) {

        /*在看门狗重置时绕过解锁检查，并在重置之前已解锁的情况下允许在飞行中解锁，

        这允许在BVLOS（超视距）飞行中发生重置时返回起飞点，

        如果操作员可以通过遥测命令进行解锁。*/

        gcs().send_text(MAV_SEVERITY_WARNING, "watchdog: Bypassing arming checks");

        return true;

    }

    // 调用父类检查。

    return AP_Arming::arm_checks(method);

}

/*

  更新电机状态参数

*/

void AP_Arming_Plane::change_arm_state(void)

{

    update_soft_armed();

#if HAL_QUADPLANE_ENABLED

    plane.quadplane.set_armed(hal.util->get_soft_armed());

#endif

}

bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks)

{

    if (!AP_Arming::arm(method, do_arming_checks)) {

        return false;

    }

    if (plane.update_home()) {

        /*在调用update_home后，如果EKF拒绝执行resetHeightDatum调用，home位置可能仍然与current_loc位置不同。

        如果我们正在更新home位置，那么我们希望强制将home位置设置为当前位置，以确保相对高度起飞的正确性。*/

        if (plane.ahrs.set_home(plane.current_loc)) {

            //  更新current_loc

            plane.update_current_loc();

        }

    }

    change_arm_state();

    //  delay_arming单次触发的上升沿。

    delay_arming = true;

    gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed");

    return true;

}

/*

  解锁马达

 */

bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks)

{

    if (do_disarm_checks &&

        (method == AP_Arming::Method::MAVLINK ||

         method == AP_Arming::Method::RUDDER)) {

        if (plane.is_flying()) {

            // 如果移动过程中禁止使用MAVLink解锁

            return false;

        }

    }

   

    if (do_disarm_checks && method == AP_Arming::Method::RUDDER) {

        //  该选项必须启用：

        if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {

            gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");

            return false;

        }

    }

    if (!AP_Arming::disarm(method, do_disarm_checks)) {

        return false;

    }

    if (plane.control_mode != &plane.mode_auto) {

        // 如果飞行器未处于"auto"模式，当解锁时重置任务。

        plane.mission.reset();

    }

    // 在自动油门模式中减小油门。

    plane.throttle_suppressed = plane.control_mode->does_auto_throttle();


 

    // 如果没有指定飞行模式开关，请确保飞行模式已关闭：

#if HAL_QUADPLANE_ENABLED

    if ((plane.quadplane.air_mode == AirMode::ON) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) {

        plane.quadplane.air_mode = AirMode::OFF;

    }

#endif

    //只有在成功解除解锁时才记录日志

    change_arm_state();

#if QAUTOTUNE_ENABLED

    // 如果启用并成功，则保存垂起无人机的自动调参增益。

    if (plane.control_mode == &plane.mode_qautotune) {

        plane.quadplane.qautotune.save_tuning_gains();

    } else {

        plane.quadplane.qautotune.reset();

    }

#endif


 

    // 重新初始化在"AUTO"和"GUIDED"模式下使用的速度变量，以在执行"DO_CHANGE_SPEED"指令时更新速度设置

    plane.new_airspeed_cm = -1;

   

    gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");

    return true;

}

void AP_Arming_Plane::update_soft_armed()

{

    bool _armed = is_armed();

#if HAL_QUADPLANE_ENABLED

    if (plane.quadplane.motor_test.running){

        _armed = true;

    }

#endif

    _armed = _armed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;

    hal.util->set_soft_armed(_armed);

    AP::logger().set_vehicle_armed(hal.util->get_soft_armed());

    // update delay_arming oneshot

    // 更新延迟解锁delay_arming参数

    if (delay_arming &&

        (AP_HAL::millis() - hal.util->get_last_armed_change() >= AP_ARMING_DELAY_MS)) {

        delay_arming = false;

    }

}

/*

  额外飞机任务检查

 */

bool AP_Arming_Plane::mission_checks(bool report)

{

    // 基础检查

    bool ret = AP_Arming::mission_checks(report);

    if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {

        ret = false;

        check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");

    }

#if HAL_QUADPLANE_ENABLED

    if (plane.quadplane.available()) {

        const uint16_t num_commands = plane.mission.num_commands();

        AP_Mission::Mission_Command prev_cmd {};

        for (uint16_t i=1; i<num_commands; i++) {

            AP_Mission::Mission_Command cmd;

            if (!plane.mission.read_cmd_from_storage(i, cmd)) {

                break;

            }

            if ((cmd.id == MAV_CMD_NAV_VTOL_LAND || cmd.id == MAV_CMD_NAV_LAND) &&

                prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {

                const float dist = cmd.content.location.get_distance(prev_cmd.content.location);

                const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();

                const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise_cm*0.01;

                const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed));

                if (dist < min_dist) {

                    ret = false;

                    check_failed(ARMING_CHECK_MISSION, report, "VTOL land too short, min %.0fm", min_dist);

                }

            }

            prev_cmd = cmd;

        }

    }

#endif

    return ret;

}

天津华兴通盛航空科技有限公司
关注
  • 1
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

天津华兴通盛航空科技有限公司

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值