Copter: check that climb rate is low in landing detector

this fixes an issue where a vehicle may still be descending rapidly
and trigger the landing detector. See the log for Robs heli.
This commit is contained in:
Andrew Tridgell 2016-08-15 16:34:18 +10:00
parent 3041a75798
commit a67b9372fd
1 changed files with 4 additions and 1 deletions

View File

@ -68,7 +68,10 @@ void Copter::update_land_detector()
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);
if (motor_at_lower_limit && accel_stationary) {
// check that vertical speed is within 1m/s of zero
bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100;
if (motor_at_lower_limit && accel_stationary && descent_rate_low) {
// 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++;