函数: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;
}
敬请期待。。。。。