APM电机故障检测代码解析-预判断

概述

APM代码中thrust_loss_check()函数对电机动力损失估计使能进行了预判断，结合动力分配中的分配计算处理（后续进行详细介绍）完成电机动力损失通道的估计，本文对thrust_loss_check()函数进行简要的注释介绍。

// check for loss of thrust and trigger thrust boost in motors library
void Copter::thrust_loss_check()
{
    static uint16_t thrust_loss_counter;  // number of iterations vehicle may have been crashed

    // no-op if suppresed by flight options param
    // 参数检查：确认是否使能此项检查
    if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) {
        return;
    }

    // exit immediately if thrust boost is already engaged
    // 当前状态确认:避免重复检查
    if (motors->get_thrust_boost()) {
        return;
    }

    // return immediately if disarmed
    // 锁定 or 降落完成：未起飞，不用检查
    if (!motors->armed() || ap.land_complete) {
        thrust_loss_counter = 0;
        return;
    }

    // exit immediately if in standby
    // 准备阶段，无需检查
    if (standby_active) {
        return;
    }

    // check for desired angle over 15 degrees
    // todo: add thrust angle to AC_AttitudeControl
    // 注意此处的单位是centidegree
    // 目标倾斜角>15度：存在较大的姿态角指令，可能会出现电机平衡较差的情况
    const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
    if (sq(angle_target.x) + sq(angle_target.y) > sq(THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD)) {
        thrust_loss_counter = 0;
        return;
    }

    // check for throttle over 90% or throttle saturation
    // 油门<0.9 && 未出现油门上饱和
    if ((attitude_control->get_throttle_in() < THRUST_LOSS_CHECK_MINIMUM_THROTTLE) && (!motors->limit.throttle_upper)) {
        thrust_loss_counter = 0;
        return;
    }

    // check throttle is over 25% to prevent checks triggering from thrust limitations caused by low commanded throttle
    // 油门低于0.25：基础油门较低，无需进行检查
    if ((attitude_control->get_throttle_in() < 0.25f)) {
        thrust_loss_counter = 0;
        return;
    }

    // check for descent
    // 下降状态
    if (!is_negative(inertial_nav.get_velocity_z())) {
        thrust_loss_counter = 0;
        return;
    }

    // check for angle error over 30 degrees to ensure the aircraft has attitude control
    // 拉力方向角度差>30度，不再进行动力损失判断切除，保证有足够的姿态控制余量
    const float angle_error = attitude_control->get_att_error_angle_deg();
    if (angle_error >= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
        thrust_loss_counter = 0;
        return;
    }

    // the aircraft is descending with low requested roll and pitch, at full available throttle, with attitude control
    // we may have lost thrust
    thrust_loss_counter++;

    // check if thrust loss for 1 second
    if (thrust_loss_counter >= (THRUST_LOSS_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
        // reset counter
        thrust_loss_counter = 0;
        AP::logger().Write_Error(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED);
        // send message to gcs
        gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor() + 1);
        // enable thrust loss handling
        // 计时器满，动力损失标志位置true
        motors->set_thrust_boost(true);
        // the motors library disables this when it is no longer needed to achieve the commanded output
    }
}

总结

电机故障检测否决项：
1）参数检查：确认是否使能此项检查；
2）当前状态确认:避免重复检查；
3）锁定 or 降落完成：未起飞，不用检查；
4）准备阶段，无需进行故障检测；
5）目标倾斜角>15度：存在较大的姿态角指令，可能会出现电机平衡较差的情况，因此避免误检测；
6）油门<0.9 && 电机输出油门未饱和，无电机通道饱和，排除故障可能；
7）油门低于0.25：基础油门较低，无需进行检查；
8）下降状态，尽快降落，不进行检测及故障切除转换；
9）拉力方向角度差>30度：保证有足够的姿态控制余量。