Ardusub源码剖析(1)——AP_Arming_Sub

代码

AP_Arming_Sub.h

cpp 复制代码

#pragma once

#include <AP_Arming/AP_Arming.h>

class AP_Arming_Sub : public AP_Arming {
public:

    AP_Arming_Sub() : AP_Arming() { }

    /* Do not allow copies */
    CLASS_NO_COPY(AP_Arming_Sub);

    bool rc_calibration_checks(bool display_failure) override;
    bool pre_arm_checks(bool display_failure) override;
    bool has_disarm_function() const;

    bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override;
    bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;

protected:
    bool ins_checks(bool display_failure) override;
};

AP_Arming_Sub.cpp

cpp 复制代码

#include "AP_Arming_Sub.h"
#include "Sub.h"

bool AP_Arming_Sub::rc_calibration_checks(bool display_failure)
{
    const RC_Channel *channels[] = {
        sub.channel_roll,
        sub.channel_pitch,
        sub.channel_throttle,
        sub.channel_yaw
    };
    return rc_checks_copter_sub(display_failure, channels);
}

bool AP_Arming_Sub::has_disarm_function() const {
    bool has_shift_function = false;
    // make sure the craft has a disarm button assigned before it is armed
    // check all the standard btn functions
    for (uint8_t i = 0; i < 16; i++) {
        switch (sub.get_button(i)->function(false)) {
            case JSButton::k_shift :
                has_shift_function = true;
                break;
            case JSButton::k_arm_toggle :
                return true;
            case JSButton::k_disarm :
                return true;
        }
    }

    // check all the shift functions if there's shift assigned
    if (has_shift_function) {
        for (uint8_t i = 0; i < 16; i++) {
            switch (sub.get_button(i)->function(true)) {
                case JSButton::k_arm_toggle :
                case JSButton::k_disarm :
                    return true;
            }
        }
    }
    return false;
}

bool AP_Arming_Sub::pre_arm_checks(bool display_failure)
{
    if (armed) {
        return true;
    }
    // don't allow arming unless there is a disarm button configured
    if (!has_disarm_function()) {
        check_failed(display_failure, "Must assign a disarm or arm_toggle button");
        return false;
    }

    return AP_Arming::pre_arm_checks(display_failure);
}

bool AP_Arming_Sub::ins_checks(bool display_failure)
{
    // call parent class checks
    if (!AP_Arming::ins_checks(display_failure)) {
        return false;
    }

    // additional sub-specific checks
    if (check_enabled(ARMING_CHECK_INS)) {
        char failure_msg[50] = {};
        if (!AP::ahrs().pre_arm_check(false, failure_msg, sizeof(failure_msg))) {
            check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);
            return false;
        }
    }

    return true;
}

bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
{
    static bool in_arm_motors = false;

    // exit immediately if already in this function
    if (in_arm_motors) {
        return false;
    }

    in_arm_motors = true;

    if (!AP_Arming::arm(method, do_arming_checks)) {
        AP_Notify::events.arming_failed = true;
        in_arm_motors = false;
        return false;
    }

#if HAL_LOGGING_ENABLED
    // let logger know that we're armed (it may open logs e.g.)
    AP::logger().set_vehicle_armed(true);
#endif

    // disable cpu failsafe because initialising everything takes a while
    sub.mainloop_failsafe_disable();

    // notify that arming will occur (we do this early to give plenty of warning)
    AP_Notify::flags.armed = true;
    // call notify update a few times to ensure the message gets out
    for (uint8_t i=0; i<=10; i++) {
        AP::notify().update();
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    send_arm_disarm_statustext("Arming motors");
#endif

    AP_AHRS &ahrs = AP::ahrs();

    sub.initial_armed_bearing = ahrs.yaw_sensor;

    if (!ahrs.home_is_set()) {
        // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)

        // Always use absolute altitude for ROV
        // ahrs.resetHeightDatum();
        // AP::logger().Write_Event(LogEvent::EKF_ALT_RESET);
    } else if (!ahrs.home_is_locked()) {
        // Reset home position if it has already been set before (but not locked)
        if (!sub.set_home_to_current_location(false)) {
            // ignore this failure
        }
    }

    hal.util->set_soft_armed(true);

    // enable output to motors
    sub.enable_motor_output();

    // finally actually arm the motors
    sub.motors.armed(true);

#if HAL_LOGGING_ENABLED
    // log flight mode in case it was changed while vehicle was disarmed
    AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason);
#endif

    // reenable failsafe
    sub.mainloop_failsafe_enable();

    // perf monitor ignores delay due to arming
    AP::scheduler().perf_info.ignore_this_loop();

    // flag exiting this function
    in_arm_motors = false;

    // if we do not have an ekf origin then we can't use the WMM tables
    if (!sub.ensure_ekf_origin()) {
        gcs().send_text(MAV_SEVERITY_WARNING, "Compass performance degraded");
        if (check_enabled(ARMING_CHECK_PARAMETERS)) {
            check_failed(ARMING_CHECK_PARAMETERS, true, "No world position, check ORIGIN_* parameters");
            return false;
        }
    }
    // return success
    return true;
}

bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks)
{
    // return immediately if we are already disarmed
    if (!sub.motors.armed()) {
        return false;
    }

    if (!AP_Arming::disarm(method, do_disarm_checks)) {
        return false;
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    send_arm_disarm_statustext("Disarming motors");
#endif

    auto &ahrs = AP::ahrs();

    // save compass offsets learned by the EKF if enabled
    if (ahrs.use_compass() && AP::compass().get_learn_type() == Compass::LEARN_EKF) {
        for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
            Vector3f magOffsets;
            if (ahrs.getMagOffsets(i, magOffsets)) {
                AP::compass().set_and_save_offsets(i, magOffsets);
            }
        }
    }

    // send disarm command to motors
    sub.motors.armed(false);

    // reset the mission
    sub.mission.reset();

#if HAL_LOGGING_ENABLED
    AP::logger().set_vehicle_armed(false);
#endif

    hal.util->set_soft_armed(false);

    // clear input holds
    sub.clear_input_hold();

    return true;
}

解析

头文件

.h代码定义了一个名为 AP_Arming_Sub 的 C++ 类，它继承自 AP_Arming 类。此类提供了一个子类化 AP_Arming 的例子，其中添加了一些基类的方法。以下是详细解释：

cpp 复制代码

#pragma once

#pragma once 是一个预处理器指令，用于防止文件被多次包含。当编译器看到这个指令时，它会确保该文件在一个编译过程中只被包含一次，即使它被多次 #include。

cpp 复制代码

#include <AP_Arming/AP_Arming.h>

这行代码包含了 AP_Arming 类定义的头文件。AP_Arming 是一个库负责处理无人机启动和关闭相关的逻辑。

cpp 复制代码

class AP_Arming_Sub : public AP_Arming {

这里定义了一个新的类 AP_Arming_Sub，它继承自 AP_Arming 类。public 关键字表示 AP_Arming 是一个公共基类。

cpp 复制代码

public:

    AP_Arming_Sub() : AP_Arming() { }

这部分定义了一个公共构造函数 AP_Arming_Sub。构造函数使用成员初始化列表语法 : AP_Arming() 来调用基类 AP_Arming 的构造函数。

cpp 复制代码

    /* Do not allow copies */
    CLASS_NO_COPY(AP_Arming_Sub);

这一行是一个注释，说明了不希望这个类被复制。虽然 CLASS_NO_COPY 宏在这里没有被定义，但它通常用于在类定义中禁用复制构造函数和赋值操作符，以防止类的无意复制。

cpp 复制代码

    bool rc_calibration_checks(bool display_failure) override;
    bool pre_arm_checks(bool display_failure) override;
    bool has_disarm_function() const;

这些是 AP_Arming_Sub 类的公共成员函数声明。它们覆盖了基类 AP_Arming 中的虚函数。override 关键字明确表示这些函数是覆盖基类的虚函数。
- rc_calibration_checks：用于执行遥控器校准检查。
- pre_arm_checks：用于执行武器启动前的检查。
- has_disarm_function：用于检查是否解锁。

cpp 复制代码

    bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override;
    bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;

这些函数覆盖了基类中的武器解除和武器启动功能，允许子类自定义这些行为。Method 是一个属于 AP_Arming 类的枚举类型，用于指定解除或启动武器的方法。

cpp 复制代码

protected:
    bool ins_checks(bool display_failure) override;

这部分声明了一个受保护的成员函数 ins_checks，它覆盖了基类中的虚函数。protected 关键字表示这个函数是供派生类和基类内部使用的，但不供外部访问。这个函数用于执行惯性导航系统（INS）检查。

源文件

cpp 复制代码

#include "AP_Arming_Sub.h"
#include "Sub.h"

这两行包含了必要的头文件，以便编译器知道 AP_Arming_Sub 类和 Sub 类（或结构体）的定义。
"AP_Arming_Sub.h" 是包含 AP_Arming_Sub 类声明的头文件。
"Sub.h" 包含一个名为 Sub 的类或结构体的定义，这个 Sub 类（以下称为"子类"）代表一个特定的无人机子系统的配置。

AP_Arming_Sub::rc_calibration_checks(bool display_failure)

cpp 复制代码

bool AP_Arming_Sub::rc_calibration_checks(bool display_failure)
{

这里定义了 AP_Arming_Sub 类的 rc_calibration_checks 成员函数。此函数接受一个布尔参数 display_failure，它指示是否应该显示校准失败的信息。

cpp 复制代码

    const RC_Channel *channels[] = {
        sub.channel_roll,
        sub.channel_pitch,
        sub.channel_throttle,
        sub.channel_yaw
    };

这里定义了一个 RC_Channel 类型的指针数组 channels，它包含指向四个不同通道的指针。这些通道通常代表多旋翼或无人机的遥控器控制通道：
- channel_roll：横滚通道，控制无人机左右倾斜。
- channel_pitch：俯仰通道，控制无人机前后倾斜。
- channel_throttle：油门通道，控制无人机的升力。
- channel_yaw：偏航通道，控制无人机的旋转方向。
sub 是一个 Sub 类型的对象，它包含了这些通道的引用或指针。

cpp 复制代码

    return rc_checks_copter_sub(display_failure, channels);
}

这行代码调用了 rc_checks_copter_sub 函数（这个函数可能在当前文件或其他文件中定义），并将 display_failure 和 channels 数组作为参数传递。这个函数的目的是执行具体的遥控器校准检查，并返回一个布尔值，指示检查是否通过。
返回值会被传递回调用 rc_calibration_checks 的地方，指示校准检查是否成功。

AP_Arming_Sub::has_disarm_function() const

cpp 复制代码

bool AP_Arming_Sub::has_disarm_function() const {

has_disarm_function 是 AP_Arming_Sub 类的一个 const 成员函数，这意味着它不会修改类的任何成员变量。const 关键字用在成员函数的声明和定义中，以确保函数调用不会改变对象的状态。

cpp 复制代码

    bool has_shift_function = false;

定义一个布尔变量 has_shift_function，用于跟踪是否存在"shift"功能按钮。

cpp 复制代码

    // make sure the craft has a disarm button assigned before it is armed
    // check all the standard btn functions
    for (uint8_t i = 0; i < 16; i++) {
        switch (sub.get_button(i)->function(false)) {
            case JSButton::k_shift :
                has_shift_function = true;
                break;
            case JSButton::k_arm_toggle :
                return true;
            case JSButton::k_disarm :
                return true;
        }
    }

这段代码遍历前16个按钮（假设设备有16个按钮），检查每个按钮的标准功能（非"shift"功能）。
sub.get_button(i) 获取第 i 个按钮对象的指针。
function(false) 调用按钮对象的 function 方法，传递 false 表示不检查"shift"功能。
如果按钮的功能是 JSButton::k_shift，则设置 has_shift_function 为 true。
如果按钮的功能是 JSButton::k_arm_toggle 或 JSButton::k_disarm，则直接返回 true，因为这表示存在解除武器功能。

cpp 复制代码

    // check all the shift functions if there's shift assigned
    if (has_shift_function) {
        for (uint8_t i = 0; i < 16; i++) {
            switch (sub.get_button(i)->function(true)) {
                case JSButton::k_arm_toggle :
                case JSButton::k_disarm :
                    return true;
            }
        }
    }

如果检测到有 shift 功能按钮，则再次遍历所有按钮，这次检查它们的"shift"功能。
function(true)这次传递 true 来检查按钮的"shift"功能。
如果任何一个按钮的"shift"功能是 JSButton::k_arm_toggle 或 JSButton::k_disarm，则返回 true。

cpp 复制代码

    return false;
}

如果遍历所有按钮后没有找到解除武器功能，则返回 false。

AP_Arming_Sub::pre_arm_checks(bool display_failure)

cpp 复制代码

bool AP_Arming_Sub::pre_arm_checks(bool display_failure)

bool: 表示这个函数返回一个布尔类型的值，即true或false。
AP_Arming_Sub::: 指定pre_arm_checks函数是AP_Arming_Sub类的成员函数。
pre_arm_checks: 函数名，意味着执行预启动检查。
(bool display_failure): 函数接收一个名为display_failure的布尔型参数，这个参数用来决定是否显示检查失败的信息。

cpp 复制代码

{
    if (armed) {
        return true;
    }

这段代码检查一个名为armed的变量（一个成员变量，表示系统是否已经启动）。
如果系统已经启动（armed为true），则函数直接返回true，不需要进行进一步的检查。

cpp 复制代码

    // don't allow arming unless there is a disarm button configured
    if (!has_disarm_function()) {
        check_failed(display_failure, "Must assign a disarm or arm_toggle button");
        return false;
    }

这部分代码检查是否存在一个配置好的解锁按钮。
!has_disarm_function(): 如果has_disarm_function()函数返回false（表示没有配置解锁按钮），则执行以下代码。
check_failed(display_failure, "Must assign a disarm or arm_toggle button"): 调用一个名为check_failed的函数，它用来记录错误信息或显示错误消息。传递的字符串说明了错误的原因。
return false;: 如果没有配置解锁按钮，则返回false，表示预检查失败。

cpp 复制代码

    return AP_Arming::pre_arm_checks(display_failure);
}

这行代码调用基类AP_Arming的pre_arm_checks函数，继续执行进一步的预启动检查。
如果这个基类的预检查也返回true，则整个pre_arm_checks函数返回true，否则返回false。

AP_Arming_Sub::ins_checks(bool display_failure)

cpp 复制代码

bool AP_Arming_Sub::ins_checks(bool display_failure)

bool: 表示这个函数返回一个布尔类型的值，即true或false。
AP_Arming_Sub::: 指定ins_checks函数是AP_Arming_Sub类的成员函数。
ins_checks: 函数名，意味着执行与INS相关的预启动检查。
(bool display_failure): 函数接收一个名为display_failure的布尔型参数，用来决定是否显示检查失败的信息。

cpp 复制代码

{
    // call parent class checks
    if (!AP_Arming::ins_checks(display_failure)) {
        return false;
    }

这段代码调用基类AP_Arming的ins_checks函数，执行与INS相关的通用预启动检查。
如果基类的检查返回false，表示检查失败，那么这个函数也立即返回false。

cpp 复制代码

    // additional sub-specific checks
    if (check_enabled(ARMING_CHECK_INS)) {

这段代码检查是否启用了特定的检查项ARMING_CHECK_INS。check_enabled根据传入的参数来确定是否执行后续的检查。

cpp 复制代码

        char failure_msg[50] = {};
        if (!AP::ahrs().pre_arm_check(false, failure_msg, sizeof(failure_msg))) {
            check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);
            return false;
        }
    }

char failure_msg[50] = {};: 定义一个字符数组failure_msg，用来存储可能的错误信息，大小为50个字符。
AP::ahrs().pre_arm_check(false, failure_msg, sizeof(failure_msg)): 调用AP::ahrs()对象的pre_arm_check方法。AP::ahrs()返回一个AHRS（Attitude and Heading Reference System）类的实例。pre_arm_check方法执行与AHRS相关的检查，如果检查失败，它将错误信息写入failure_msg数组，并返回false。
如果pre_arm_check返回false，则执行以下代码：
- check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);: 调用check_failed函数，它用于记录错误信息或显示错误消息。它接收检查项标识符ARMING_CHECK_INS，是否显示失败的标志display_failure，以及格式化的错误信息字符串，其中%s将被failure_msg中的内容替换。
- return false;: 如果检查失败，则返回false。

cpp 复制代码

    return true;
}

如果所有的检查都通过，则函数返回true。

AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)

cpp 复制代码

bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
{
    static bool in_arm_motors = false;

声明一个静态布尔变量in_arm_motors，用于防止函数重入。

cpp 复制代码

    // exit immediately if already in this function
    if (in_arm_motors) {
        return false;
    }

如果in_arm_motors为true，表示函数已经在执行中，为了避免重入，直接返回false。

cpp 复制代码

    in_arm_motors = true;

设置in_arm_motors为true，表示当前正在执行启动过程。

cpp 复制代码

    if (!AP_Arming::arm(method, do_arming_checks)) {
        AP_Notify::events.arming_failed = true;
        in_arm_motors = false;
        return false;
    }

调用基类AP_Arming的arm方法尝试启动。如果返回false，表示启动失败，设置通知事件，重置in_arm_motors，并返回false。

cpp 复制代码

#if HAL_LOGGING_ENABLED
    // let logger know that we're armed (it may open logs e.g.)
    AP::logger().set_vehicle_armed(true);
#endif

如果启用了日志记录功能，通知日志系统车辆已启动。

cpp 复制代码

    // disable cpu failsafe because initialising everything takes a while
    sub.mainloop_failsafe_disable();

禁用主循环故障安全，因为在初始化过程中可能会有较长时间的延迟。

cpp 复制代码

    // notify that arming will occur (we do this early to give plenty of warning)
    AP_Notify::flags.armed = true;
    // call notify update a few times to ensure the message gets out
    for (uint8_t i=0; i<=10; i++) {
        AP::notify().update();
    }

设置通知标志表明车辆将要启动，并通过多次调用更新函数确保通知消息被发送。

cpp 复制代码

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    send_arm_disarm_statustext("Arming motors");
#endif

如果配置为SITL（软件在环仿真），发送状态文本消息表明正在启动电机。

cpp 复制代码

    AP_AHRS &ahrs = AP::ahrs();

获取AHRS（姿态和航向参考系统）实例的引用。

cpp 复制代码

    sub.initial_armed_bearing = ahrs.yaw_sensor;

设置初始启动时的航向。

cpp 复制代码

    if (!ahrs.home_is_set()) {
        // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
        // Always use absolute altitude for ROV
        // ahrs.resetHeightDatum();
        // AP::logger().Write_Event(LogEvent::EKF_ALT_RESET);
    } else if (!ahrs.home_is_locked()) {
        // Reset home position if it has already been set before (but not locked)
        if (!sub.set_home_to_current_location(false)) {
            // ignore this failure
        }
    }

如果家位置未设置，则重置EKF高度；如果家位置未锁定，则重置为当前位置。

cpp 复制代码

    hal.util->set_soft_armed(true);

设置软启动标志，表明系统已启动。

cpp 复制代码

    // enable output to motors
    sub.enable_motor_output();

启用电机输出。

cpp 复制代码

    // finally actually arm the motors
    sub.motors.armed(true);

最终启动电机。

cpp 复制代码

#if HAL_LOGGING_ENABLED
    // log flight mode in case it was changed while vehicle was disarmed
    AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason);
#endif

如果启用了日志记录，记录当前的飞行模式。

cpp 复制代码

    // reenable failsafe
    sub.mainloop_failsafe_enable();

重新启用主循环故障安全。

cpp 复制代码

    // perf monitor ignores delay due to arming
    AP::scheduler().perf_info.ignore_this_loop();

通知性能监视器忽略由于启动导致的延迟。

cpp 复制代码

    // flag exiting this function
    in_arm_motors = false;

标记退出此函数，将in_arm_motors重置为false。

cpp 复制代码

    // if we do not have an ekf origin then we can't use the WMM tables
    if (!sub.ensure_ekf_origin()) {
        gcs().send_text(MAV_SEVERITY_WARNING, "Compass performance degraded");
        if (check_enabled(ARMING_CHECK_PARAMETERS)) {
            check_failed(ARMING_CHECK_PARAMETERS, true, "No world position, check ORIGIN_* parameters");
            return false;
        }
    }

如果EKF原点未设置，则发送警告并检查是否启用了参数检查。如果启用了，则记录失败并返回false。

cpp 复制代码

    // return success
    return true;
}

如果所有步骤都成功完成，则返回true。

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

cpp 复制代码

bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks)
{

声明一个返回布尔值的函数disarm，它接收两个参数：解锁方法method和一个布尔值do_disarm_checks，后者指定是否执行解锁前的检查。

cpp 复制代码

    // return immediately if we are already disarmed
    if (!sub.motors.armed()) {
        return false;
    }

如果电机已经处于未启动状态（即已解锁），则立即返回false。

cpp 复制代码

    if (!AP_Arming::disarm(method, do_disarm_checks)) {
        return false;
    }

调用基类AP_Arming的disarm方法尝试解锁。如果返回false，表示解锁失败，因此直接返回false。

cpp 复制代码

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    send_arm_disarm_statustext("Disarming motors");
#endif

如果配置为SITL（软件在环仿真），发送状态文本消息表明正在解锁电机。

cpp 复制代码

    auto &ahrs = AP::ahrs();

获取AHRS（姿态和航向参考系统）实例的引用。

cpp 复制代码

    // save compass offsets learned by the EKF if enabled
    if (ahrs.use_compass() && AP::compass().get_learn_type() == Compass::LEARN_EKF) {
        for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
            Vector3f magOffsets;
            if (ahrs.getMagOffsets(i, magOffsets)) {
                AP::compass().set_and_save_offsets(i, magOffsets);
            }
        }
    }

如果启用了指南针，并且EKF学习了指南针偏移量，则保存这些偏移量。

cpp 复制代码

    // send disarm command to motors
    sub.motors.armed(false);

向电机发送解锁命令，将电机设置为未启动状态。

cpp 复制代码

    // reset the mission
    sub.mission.reset();

重置任务，可能是指飞行任务或一系列预定的动作。

cpp 复制代码

#if HAL_LOGGING_ENABLED
    AP::logger().set_vehicle_armed(false);
#endif

如果启用了日志记录功能，通知日志系统车辆已解锁。

cpp 复制代码

    hal.util->set_soft_armed(false);

更新软启动状态，表明系统已解锁。

cpp 复制代码

    // clear input holds
    sub.clear_input_hold();

清除输入保持状态，这可能是为了确保在解锁后不会有任何悬挂的输入命令影响系统。

cpp 复制代码

    return true;
}

如果解锁过程成功完成，则返回true。