Apm飞控学习笔记之-电机解锁和故障保护-Cxm

Posted 2023-02-16 CHENxiaomingming

前言

CSDNhttps://mp.csdn.net/mp_blog/creation/editor/122115245

这边说的解锁是指的非自动解锁和地面站解锁

常规解锁流程=安全开关->内八解锁

这一篇会介绍整个解锁流程以及飞控内部解锁包含了那些内容

---

目录

是否允许使用舵面解锁

判断油门是否关闭

获取舵量.航向

 主体内容

 主题内容中关注的函数

1.motors->armed()   

2.arming.arm(AP_Arming::Method::RUDDER)

打开libraries\\AP_Arming\\AP_Arming.cpp文件中的arm函数

((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true)&& arm_checks(method)))

pre_arm_checks(true)

---

ArduCopter\\motors.cpp

飞控的解锁的文件就在这里

命名为arm_motors_check()函数

内容包含非常多 所以就不一一列出了 我按顺序讲

可以看到这个函数在主文件里开辟了进程

然后我们直接来看这个函数的流程

是否允许使用舵面解锁

AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();

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

return; 

判断油门是否关闭

    if (channel_throttle->get_control_in() > 0) 
        arming_counter = 0;
        return;
    

获取舵量.航向

int16_t yaw_in = channel_yaw->get_control_in();

 主体内容

if (yaw_in > 4000) 

        // increase the arming counter to a maximum of 1 beyond the auto trim counter
        //最大值1 如果小于就等于1
        if (arming_counter <= AUTO_TRIM_DELAY) 
            arming_counter++;
        
        // arm the motors and configure for flight
        //准备发动机准备起飞
        if (arming_counter == ARM_DELAY && !motors->armed()) 
            // reset arming counter if arming fail
            //解除失败就重新解除
            //关键解锁函数arming.arm(AP_Arming::Method::RUDDER)
            if (!arming.arm(AP_Arming::Method::RUDDER)) 
                arming_counter = 0;
            
        

        // arm the motors and configure for flight
        //启动发动机，准备起飞 
        if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::STABILIZE) 
            auto_trim_counter = 250;
            // ensure auto-disarm doesn't trigger immediately
            //确保不会第一时间
            auto_disarm_begin = millis();
        

    // full left and rudder disarming is enabled
    //左满舵解除武装
     else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) 

        if (!flightmode->has_manual_throttle() && !ap.land_complete) 
            arming_counter = 0;
            return;
        

        // increase the counter to a maximum of 1 beyond the disarm delay
        if (arming_counter <= DISARM_DELAY) 
            arming_counter++;
        

        // disarm the motors
        //解除引擎
        if (arming_counter == DISARM_DELAY && motors->armed()) 
            //解锁函数
            arming.disarm();
        

    // Yaw is centered so reset arming counter
     else 
        arming_counter = 0;
    

 主题内容中关注的函数

arming.arm(AP_Arming::Method::RUDDER)

注意这里要关注的函数是ArduCopter\\motors.cpp中的:

arming.arm(AP_Arming::Method::RUDDER)

而这个函数是在同目录下的ArduCopter\\AP_Arming.cpp文件中

这里一定要注意千万不要弄混了

我们跳转到这个函数里面,注意这个函数是在同目录下的ArduCopter\\AP_Arming.cpp文件中

bool AP_Arming_Copter::arm(const AP_Arming::Method method, const 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;

    // return true if already armed
    //如果已经启动
    if (copter.motors->armed()) 
        in_arm_motors = false;
        return true;
    


    //AP_Arming::arm 是整个飞机的模式和防护 如果成功返回t
    //这个很重要 这里面包括了里面的检查项目 包含了pre_arm_checks(true)
    if (!AP_Arming::arm(method, do_arming_checks)) 
        AP_Notify::events.arming_failed = true;
        in_arm_motors = false;
        return false;
    
    


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

    // disable cpu failsafe because initialising everything takes a while
    copter.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 HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
    gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif

    // Remember Orientation
    // --------------------
    copter.init_simple_bearing();

    AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();

    copter.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)
        //home 点设置
        ahrs.resetHeightDatum();
        AP::logger().Write_Event(DATA_EKF_ALT_RESET);

        // we have reset height, so arming height is zero
        copter.arming_altitude_m = 0;
     else if (!ahrs.home_is_locked()) 
        // Reset home position if it has already been set before (but not locked)
        //重置初始位置（如果之前已设置）（但未锁定）
        if (!copter.set_home_to_current_location(false)) 
            // ignore failure
        

        // remember the height when we armed
        //记住我们武装时的高度
        copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01;
    
    更新超级简单轴承-根据位置调整简单轴承
    //应在home_轴承更新后调用
    copter.update_super_simple_bearing(false);

    // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
#if MODE_SMARTRTL_ENABLED == ENABLED
    copter.g2.smart_rtl.set_home(copter.position_ok());
#endif

    // enable gps velocity based centrefugal force compensation
    ahrs.set_correct_centrifugal(true);
    hal.util->set_soft_armed(true);

#if SPRAYER_ENABLED == ENABLED
    // turn off sprayer's test if on
    copter.sprayer.test_pump(false);
#endif

    // enable output to motors
    //启用电机的输出
    copter.enable_motor_output();

    // finally actually arm the motors
    //启动发动机
    copter.motors->armed(true);

    AP::logger().Write_Event(DATA_ARMED);

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

    // re-enable failsafe
    copter.failsafe_enable();

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

    // flag exiting this function
    in_arm_motors = false;

    // Log time stamp of arming event
    copter.arm_time_ms = millis();

    // Start the arming delay
    copter.ap.in_arming_delay = true;

    // assumed armed without a arming, switch. Overridden in switches.cpp
    copter.ap.armed_with_switch = false;

    // return success
    return true;

1.motors->armed()   

检查当前是否解锁 bool类型 1解锁 0未解锁 注意这个函数是个重载函数里面是可以包含值的

    if (copter.motors->armed()) 
        in_arm_motors = false;
        return true;
    

2.arming.arm(AP_Arming::Method::RUDDER)

这个是重点关注的函数 也就是解锁的函数 他很重要,在libraries\\AP_Arming\\AP_Arming.cpp里面定义了一个一样函数名的函数 这点要注意两个函数的功能不同

接下来不重要的部分我直接吧作用放在注释里

    //如果已经在函数中
    if (in_arm_motors) 
        return false;
    

  //如果已经启动
    if (copter.motors->armed()) 
        in_arm_motors = false;
        return true;
    

注意这里到了前面说的嵌套AP_Arming::arm(method, do_arming_checks)

这个函数的主题在libraries\\AP_Arming\\AP_Arming.cpp文件中

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

注意在这里我们跳转到 libraries\\AP_Arming\\AP_Arming.cpp中的

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

前一个同名函数是为了解锁

后者则是为了检查当前飞机的状态然后更改电机启动标志位

打开libraries\\AP_Arming\\AP_Arming.cpp文件中的arm函数

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

    if (armed)  //already armed
        return false;
    

    if ((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true) && arm_checks(method))) 
        armed = true;

        //TODO: Log motor arming
        //Can't do this from this class until there is a unified logging library

     else 
        //解除失败日志通知
        AP::logger().arming_failure();
        armed = false;
    

    return armed;

其中核心就是

((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true)&& arm_checks(method)))

pre_arm_checks(true)

飞行前的检查

bool AP_Arming_Copter::pre_arm_checks(bool display_failure)

    const bool passed = run_pre_arm_checks(display_failure);
    set_pre_arm_check(passed);
    return passed;

我们要重点关注的是

run_pre_arm_checks(display_failure)

bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)

    // exit immediately if already armed
    if (copter.motors->armed()) 
        return true;
    


    // check if motor interlock and Emergency Stop aux switches are used
    // at the same time.  This cannot be allowed.
    if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) &&
        rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP))
        check_failed(display_failure, "Interlock/E-Stop Conflict");
        return false;
    

    // check if motor interlock aux switch is in use
    // if it is, switch needs to be in disabled position to arm
    // otherwise exit immediately.  This check to be repeated,
    // as state can change at any time.
    if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) 
        check_failed(display_failure, "Motor Interlock Enabled");
    

    // if pre arm checks are disabled run only the mandatory checks
    if (checks_to_perform == 0) 
        return mandatory_checks(display_failure);
    

    return fence_checks(display_failure)        //围栏
        & parameter_checks(display_failure)     //参数
        & motor_checks(display_failure)         //电机
        & pilot_throttle_checks(display_failure)    //油门
        & oa_checks(display_failure)
        & gcs_failsafe_check(display_failure) &     //地面站故障保护
        AP_Arming::pre_arm_checks(display_failure);

最重要的部分这边我都已经打上了注释如果感兴趣可以自己去查看这边关注一下油门

bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)如下:

bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)

    // check throttle is above failsafe throttle
    // this is near the bottom to allow other failures to be displayed before checking pilot throttle
    //检查油门是否高于故障保护油门
    //这靠近底部，以便在检查先导油门之前显示其他故障
    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) 
        if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) 
            #if FRAME_CONFIG == HELI_FRAME
            const char *failmsg = "Collective below Failsafe";
            #else
            const char *failmsg = "Throttle below Failsafe";
            #endif
            check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg);
            return false;
        
    

    return true;

大概的意思就是查看标志位是否有其他故障 然后检查油门位置 这个函数会决定是否能在未开启遥控器的情况下解锁