旋翼电机停转保护测试时， gazebo仿真和实际飞行的区别
1、gazebo仿真过程中， 模拟了esc的电流电压和转速等数据。 而实际飞行时必须依靠esc电调返回的电流值。

通过设置CA_MOTOR_FAIL_EN = 1， CA_MOTOR_FAIL_ID = 5使5号电机停转。
电机停转后， 使用listener命令查看esc_status内容。
在nsh中执行listener esc_status， 重点看esc[5]

TOPIC: esc_status
 esc_status
    timestamp: 848960000 (0.000000 seconds ago)
    counter: 0
    esc_count: 8
    esc_connectiontype: 0
    esc_online_flags: 255 (0b1111'1111)
    esc_armed_flags: 255 (0b1111'1111)

  esc[0] (esc_report):
    timestamp: 848960000 (0.000000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 3456
    esc_voltage: 15.30000
    esc_current: 9.64000
    esc_temperature: 43.04000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 101
    esc_power: 0

  esc[1] (esc_report):
    timestamp: 848960000 (0.000000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 2934
    esc_voltage: 15.30000
    esc_current: 8.33500
    esc_temperature: 39.56000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 102
    esc_power: 0

  esc[2] (esc_report):
    timestamp: 848960000 (0.000000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 3420
    esc_voltage: 15.30000
    esc_current: 9.55000
    esc_temperature: 42.80000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 103
    esc_power: 0

  esc[3] (esc_report):
    timestamp: 848960000 (0.000000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 2970
    esc_voltage: 15.30000
    esc_current: 8.42500
    esc_temperature: 39.80000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 104
    esc_power: 0

  esc[4] (esc_report):
    timestamp: 848960000 (0.000000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 0
    esc_voltage: 15.30000
    esc_current: 1.00000
    esc_temperature: 20.00000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 105
    esc_power: 0

  esc[5] (esc_report):
    timestamp: 848960000 (0.000000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 1740
    esc_voltage: 15.30000
    esc_current: 5.35000
    esc_temperature: 31.60000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 106
    esc_power: 0

  esc[6] (esc_report):
    timestamp: 848960000 (0.004000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 0
    esc_voltage: 15.30000
    esc_current: 1.00000
    esc_temperature: 20.00000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 164
    esc_power: 0

  esc[7] (esc_report):
    timestamp: 848960000 (0.004000 seconds ago)
    esc_errorcount: 0
    esc_rpm: 0
    esc_voltage: 15.30000
    esc_current: 1.00000
    esc_temperature: 20.00000
    failures: 0
    esc_address: 0
    esc_cmdcount: 0
    esc_state: 0
    actuator_function: 165
    esc_power: 0

pxh> 

审查代码后， 发现这么一段代码， 我这里贴出来。
/home/cuigaosheng/AirVerse/src/modules/commander/failure_detector

bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
{
        _failure_injector.update();

        failure_detector_status_u status_prev = _status;

        if (vehicle_control_mode.flag_control_attitude_enabled) {
                updateAttitudeStatus(vehicle_status);

                if (_param_fd_ext_ats_en.get()) {
                        updateExternalAtsStatus();
                }

        } else {
                _status.flags.roll = false;
                _status.flags.pitch = false;
                _status.flags.alt = false;
                _status.flags.ext = false;
        }
//搜寻遍整个代码之后发现，updateMotorStatus（vehicle_status， esc_status）这个函数只在这里被调用，而被调用的前提竟然是 if (_esc_status_sub.update(&esc_status)),  那么问题就来了， 实际飞行的时候，并没有esc设备， 也就不会有esc_status的数据流， 这个条件将永远都不会被满足。 所以updateMotorStatus(vehicle_status, esc_status)这个函数永远不会被执行。  
#if 0
        // esc_status subscriber is shared between subroutines
        esc_status_s esc_status;

        if (_esc_status_sub.update(&esc_status)) {
                _failure_injector.manipulateEscStatus(esc_status);

                if (_param_escs_en.get()) {
                        updateEscsStatus(vehicle_status, esc_status);
                }

                if (_param_fd_actuator_en.get()) {
                        updateMotorStatus(vehicle_status, esc_status);
                }
        }
#endif

        if(!_param_fd_esc_sim_en.get())
        {
                if (_param_fd_actuator_en.get()) {

                        esc_status_s esc_status;
                        updateMotorStatus(vehicle_status, esc_status);
                }
        }

        if (_param_fd_imb_prop_thr.get() > 0) {
                updateImbalancedPropStatus();
        }

        return _status.value != status_prev.value;
}

如果updateMotorStatus(vehicle_status, esc_status)函数不能被运行的话。 下面的代码也将不能正常运行， 也就出现了这么一个状况， 当我listener failure_detector_status的时候， 我看到fd_motor = true, 但是listener vehicle_status的时候，其中的motor却并不能实时被更新。

    failure_detector_status_s external_fd_status;
        if(_failure_detector_status_sub.update(&external_fd_status))
        {

                if(external_fd_status.fd_motor)
                {
                        _status.flags.motor = true;
                        return;
                }
                else
                {
                        _status.flags.motor = false;
                        return;
                }
        }

上面的这段代码， 在FailureDetector.cpp文件中， 主要做的工作是将failure_detector_status_s中的fd_motor = true， 赋值给到_status.falgs.motor = true.
从msg/FailureDetectorStatus.msg我们看到了下面的消息。

uint64 timestamp                    # time since system start (microseconds)

# FailureDetector status
bool fd_roll
bool fd_pitch
bool fd_alt
bool fd_ext
bool fd_arm_escs
bool fd_battery
bool fd_imbalanced_prop
bool fd_motor

float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)
uint16 motor_failure_mask           # Bit-mask with motor indices, indicating critical motor failures

现在再来看看_status.flags.motor是从哪里来的。 在FailureDetector.hpp文件中有下面的内容。

union failure_detector_status_u {
        struct {
                uint16_t roll : 1;
                uint16_t pitch : 1;
                uint16_t alt : 1;
                uint16_t ext : 1;
                uint16_t arm_escs : 1;
                uint16_t battery : 1;
                uint16_t imbalanced_prop : 1;
                uint16_t motor : 1;
        } flags;
        uint16_t value {0};
};

当fd_motor的值是true的时候， 执行了下面的命令， 那么将会出现0x80这么个值
_status.flags.motor = true;
我们紧接着再来看看
/home/cuigaosheng/AirVerse/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp

      
void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter)
{
		reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
        if (reporter.failsafeFlags().fd_motor_failure) {
                reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_motor"),
                
                events::Log::Critical, "Motor failure detected");
        }

}
       

从上面的代码来看， 假如context.status().failure_detector_status = 0x80时， reporter.failsafeFlags().fd_motor_failure才能等于true。
最终触发电机失效动作。

我现在来看看checkAndReport()函数是怎么被调用的。
FailureDetectorChecks::checkAndReport函数的调用链如下：
调用链分析
主循环: Commander::run() - 这是Commander模块的主运行循环，以高频运行（通常是100Hz）
健康检查更新: 在Commander::run()中，每100ms（10Hz）调用一次：

transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
{
							//略
                _health_and_arming_checks.update();
               //略
        return TRANSITION_CHANGED;
}

bool HealthAndArmingChecks::update(bool force_reporting)
{
        if (reported) {

               //略

                for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
                        if (!_checks[i]) {
                                break;
                        }

                        _checks[i]->checkAndReport(_context, _reporter);
                }

               //略

        return reported;
}

调用链分析完毕。

总结
FailureDetectorChecks::checkAndReport 是在 Commander模块的主循环中每100ms（10Hz） 被调用的。这个函数负责检查vehicle_status.failure_detector_status中的电机故障标志位，并相应地设置失效保护标志。
调用频率是10Hz，这意味着如果电机故障状态发生变化，最多需要100ms才能被检测到并触发相应的失效保护机制。

在测试过程中，发现上面的额代码在工作过程中， 总是会出现一些莫名奇妙的问题，
1、比如增稳状态只解锁， 遥控器油门都没加， 结果电机输出拉到了最满，
2、断桨算法运行过程中， 所有电机的输出莫名其妙突然降低，导致飞机坠落。
3、飞机接近停止的时候，停止的那个电机的对向电机，在0和1之间来回跳动。

鉴于上面的原因， 我将check_for_value恢复到原来的状态， 将模拟电机停机的代码放在了failure_detector_status的发布端。

我来理一下思路，
首先Failuredetector.cpp文件里设置了.motor = true, 然后， Commaner.cpp中.motor被赋值给了fail_detector_status, control_actuator.cpp文件中， 订阅fail_detector_status中的fd_motor值， 更新电机控制矩阵，控制飞机。 而failureDetectorCheck.cpp文件中监测.motor值，然后设置电机停止后的飞行模式。 所以这里的源头从头到尾都是一个updateMotorStatus()中的.motor。 这个就是牛鼻子。