Ardusub源码剖析(1)——AP_Arming_Sub

代码

AP_Arming_Sub.h

#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

#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 functionsfor (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 assignedif (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 configuredif (!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 checksif (!AP_Arming::ins_checks(display_failure)) {return false;}// additional sub-specific checksif (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 functionif (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 whilesub.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 outfor (uint8_t i=0; i<=10; i++) {AP::notify().update();}#if CONFIG_HAL_BOARD == HAL_BOARD_SITLsend_arm_disarm_statustext("Arming motors");
#endifAP_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 motorssub.enable_motor_output();// finally actually arm the motorssub.motors.armed(true);#if HAL_LOGGING_ENABLED// log flight mode in case it was changed while vehicle was disarmedAP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason);
#endif// reenable failsafesub.mainloop_failsafe_enable();// perf monitor ignores delay due to armingAP::scheduler().perf_info.ignore_this_loop();// flag exiting this functionin_arm_motors = false;// if we do not have an ekf origin then we can't use the WMM tablesif (!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 successreturn true;
}bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks)
{// return immediately if we are already disarmedif (!sub.motors.armed()) {return false;}if (!AP_Arming::disarm(method, do_disarm_checks)) {return false;}#if CONFIG_HAL_BOARD == HAL_BOARD_SITLsend_arm_disarm_statustext("Disarming motors");
#endifauto &ahrs = AP::ahrs();// save compass offsets learned by the EKF if enabledif (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 motorssub.motors.armed(false);// reset the missionsub.mission.reset();#if HAL_LOGGING_ENABLEDAP::logger().set_vehicle_armed(false);
#endifhal.util->set_soft_armed(false);// clear input holdssub.clear_input_hold();return true;
}

解析

头文件

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

#pragma once

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

#include <AP_Arming/AP_Arming.h>

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

class AP_Arming_Sub : public AP_Arming {

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

public:AP_Arming_Sub() : AP_Arming() { }

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

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

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

    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：用于检查是否解锁。

    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 类的枚举类型，用于指定解除或启动武器的方法。

protected:bool ins_checks(bool display_failure) override;

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

源文件

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

bool AP_Arming_Sub::rc_calibration_checks(bool display_failure)
{

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

    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 类型的对象，它包含了这些通道的引用或指针。

    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

bool AP_Arming_Sub::has_disarm_function() const {

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

    bool has_shift_function = false;

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

    // make sure the craft has a disarm button assigned before it is armed// check all the standard btn functionsfor (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，因为这表示存在解除武器功能。

    // check all the shift functions if there's shift assignedif (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。

    return false;
}

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

AP_Arming_Sub::pre_arm_checks(bool display_failure)

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的布尔型参数，这个参数用来决定是否显示检查失败的信息。

{if (armed) {return true;}

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

    // don't allow arming unless there is a disarm button configuredif (!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，表示预检查失败。

    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)

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的布尔型参数，用来决定是否显示检查失败的信息。

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

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

    // additional sub-specific checksif (check_enabled(ARMING_CHECK_INS)) {

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

        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。

    return true;
}

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

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

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

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

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

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

    in_arm_motors = true;

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

    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。

#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 whilesub.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 outfor (uint8_t i=0; i<=10; i++) {AP::notify().update();}

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

#if CONFIG_HAL_BOARD == HAL_BOARD_SITLsend_arm_disarm_statustext("Arming motors");
#endif

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

    AP_AHRS &ahrs = AP::ahrs();

• 获取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}}

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

    hal.util->set_soft_armed(true);

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

    // enable output to motorssub.enable_motor_output();

• 启用电机输出。

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

• 最终启动电机。

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

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

    // reenable failsafesub.mainloop_failsafe_enable();

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

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

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

    // flag exiting this functionin_arm_motors = false;

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

    // if we do not have an ekf origin then we can't use the WMM tablesif (!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。

    // return successreturn true;
}

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

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

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

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

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

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

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

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

#if CONFIG_HAL_BOARD == HAL_BOARD_SITLsend_arm_disarm_statustext("Disarming motors");
#endif

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

    auto &ahrs = AP::ahrs();

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

    // save compass offsets learned by the EKF if enabledif (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学习了指南针偏移量，则保存这些偏移量。

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

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

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

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

#if HAL_LOGGING_ENABLEDAP::logger().set_vehicle_armed(false);
#endif

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

    hal.util->set_soft_armed(false);

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

    // clear input holdssub.clear_input_hold();

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

    return true;
}

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