浅谈APM系列-----update_land_and_crash_detectors

函数：void Copter::update_land_and_crash_detectors()

位置：X:\ardupilot\ArduCopter\land_detector.cpp

// run land and crash detectors
// called at MAIN_LOOP_RATE
void Copter::update_land_and_crash_detectors()
{
    // update 1hz filtered acceleration
    Vector3f accel_ef = ahrs.get_accel_ef_blended();
    accel_ef.z += GRAVITY_MSS;
    land_accel_ef_filter.apply(accel_ef, scheduler.get_loop_period_s());//利用C++模板写的低通滤波器

    update_land_detector();

#if PARACHUTE == ENABLED
    // check parachute
    parachute_check();
#endif

    crash_check();
}

函数：void Copter::update_land_detector()

位置：X:\ardupilot\ArduCopter\land_detector.cpp

// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// 检查我们是否已经登陆并更新了ap.land完整标志
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
{
    // land detector can not use the following sensors because they are unreliable during landing
    // 着陆探测器不能使用以下传感器，因为它们在着陆过程中是不可靠的。
    // barometer altitude :                 ground effect can cause errors larger than 4m
    // EKF vertical velocity or altitude :  poor barometer and large acceleration from ground impact
    // earth frame angle or angle error :   landing on an uneven surface will force the airframe to match the ground angle
    // gyro output :                        on uneven surface the airframe may rock back an forth after landing
    // range finder :                       tend to be problematic at very short distances
    // input throttle :                     in slow land the input throttle may be only slightly less than hover

    if (!motors->armed()) {//在未解锁状态下，总是设置为着陆状态
        // if disarmed, always landed.
        set_land_complete(true);
    } else if (ap.land_complete) {//在着陆标志位设置的情况下
#if FRAME_CONFIG == HELI_FRAME
        // if rotor speed and collective pitch are high then clear landing flag
        if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->rotor_runup_complete()) {
#else
        // if throttle output is high then clear landing flag（如果油门的输出很高，清除着陆标志）
        if (motors->get_throttle() > get_non_takeoff_throttle()) {//油门值和不起飞油门值比较
#endif
            set_land_complete(false);//油门较大时，取消着陆状态
        }
    } else {

#if FRAME_CONFIG == HELI_FRAME
        // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
        bool motor_at_lower_limit = motors->limit.throttle_lower;
#else
        // check that the average throttle output is near minimum (less than 12.5% hover throttle)
        bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();//检测油门是不是最小值
#endif

        // check that the airframe is not accelerating (not falling or braking after fast forward flight（在快速向前飞行后不会下降或刹车）)
        bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);//检测机身有没有加速度（小于1m/s/s）

        // check that vertical speed is within 1m/s of zero
        bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100;//检测竖直速度是否小于1米每秒

        // if we have a healthy rangefinder only allow landing detection below 2 meters
        bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);

        if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check) {
            // landed criteria met - increment the counter and check if we've triggered
            if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*scheduler.get_loop_rate_hz()) {//检测着陆的时间
                land_detector_count++;
            } else {
                set_land_complete(true);//检测到着陆完成
            }
        } else {
            // we've sensed movement up or down so reset land_detector
            land_detector_count = 0;
        }
    }

    set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));//设置可能着陆标志
}

函数：void Copter::set_land_complete(bool b)

位置：X:\ardupilot\ArduCopter\land_detector.cpp

// set land_complete flag and disarm motors if disarm-on-land is configured
void Copter::set_land_complete(bool b)
{
    // if no change, exit immediately
    if( ap.land_complete == b )
        return;

    land_detector_count = 0;

    if(b){
        Log_Write_Event(DATA_LAND_COMPLETE);
    } else {
        Log_Write_Event(DATA_NOT_LANDED);
    }
    ap.land_complete = b;

    g2.stats.set_flying(!b);

    // tell AHRS flying state
    ahrs.set_likely_flying(!b);
    
    // trigger disarm-on-land if configured
    bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
    const bool mode_disarms_on_land = flightmode->allows_arming(false) && !flightmode->has_manual_throttle();

    if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
        init_disarm_motors();
    }
}

函数：void Copter::set_land_complete_maybe(bool b)

位置：X:\ardupilot\ArduCopter\land_detector.cpp

// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
{
    // if no change, exit immediately
    if (ap.land_complete_maybe == b)
        return;

    if (b) {
        Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
    }
    ap.land_complete_maybe = b;
}

 

绳锯木断，水滴石穿。

---

敬请期待。。。。。