diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 7aa6d154b1..a86fd37213 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -478,6 +478,9 @@ #ifndef LAND_DETECTOR_ACCEL_MAX # define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s #endif +#ifndef LAND_DETECTOR_RNGFND_TIMEOUT_SEC +# define LAND_DETECTOR_RNGFND_TIMEOUT_SEC 15.0f // number of seconds the non-rangefinder landing criteria must hold before a healthy-but-wrong rangefinder is overridden +#endif ////////////////////////////////////////////////////////////////////////////// // CAMERA TRIGGER AND CONTROL diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 9271e5e9e5..e92e2c0709 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -10,6 +10,9 @@ // counter to verify landings static uint32_t land_detector_count = 0; +// counter of consecutive loops in which every landing criterion except the rangefinder check has held +static uint32_t land_detector_rngfnd_timeout_count = 0; + // run land and crash detectors // called at MAIN_LOOP_RATE void Copter::update_land_and_crash_detectors() @@ -58,6 +61,7 @@ void Copter::update_land_detector() } else if (standby_active) { // land detector will not run in standby mode land_detector_count = 0; + land_detector_rngfnd_timeout_count = 0; } else { #if FRAME_CONFIG == HELI_FRAME @@ -95,7 +99,10 @@ void Copter::update_land_detector() rangefinder_check = true; } - if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check) { + // landing criteria that do not depend on the rangefinder + const bool inertial_landed = motor_at_lower_limit && accel_stationary && descent_rate_low; + + if (inertial_landed && 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++; @@ -106,6 +113,21 @@ void Copter::update_land_detector() // we've sensed movement up or down so reset land_detector land_detector_count = 0; } + + // a rangefinder can report a healthy but false distance while the vehicle is on the + // ground (NCT-3495), which would block landing detection indefinitely. If every + // other criterion has held for LAND_DETECTOR_RNGFND_TIMEOUT_SEC, override the + // rangefinder check and declare the landing. + if (inertial_landed && !ap.land_complete) { + if (land_detector_rngfnd_timeout_count < ((float)LAND_DETECTOR_RNGFND_TIMEOUT_SEC)*scheduler.get_loop_rate_hz()) { + land_detector_rngfnd_timeout_count++; + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "Land detect: rangefinder overridden"); + set_land_complete(true); + } + } else { + land_detector_rngfnd_timeout_count = 0; + } } set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz())); @@ -119,6 +141,7 @@ void Copter::set_land_complete(bool b) return; land_detector_count = 0; + land_detector_rngfnd_timeout_count = 0; if(b){ Log_Write_Event(DATA_LAND_COMPLETE);