From a67b9372fd779d309ad309a87f39c42db6482441 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 15 Aug 2016 16:34:18 +1000 Subject: [PATCH] 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. --- ArduCopter/land_detector.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 739add6d11..f2a0093b2a 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -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++;