diff --git a/ArduCopter/config.h b/ArduCopter/config.h index bd139ff23a..6a1ef097ed 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -382,6 +382,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_VEL_Z_MAX +# define LAND_DETECTOR_VEL_Z_MAX 1.0f // vehicle vertical velocity must be under 1m/s +#endif ////////////////////////////////////////////////////////////////////////////// // Flight mode definitions diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 95743cfafa..c075e6c9b8 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -112,7 +112,7 @@ void Copter::update_land_detector() bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar); // check that vertical speed is within 1m/s of zero - bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100 * land_detector_scalar; + bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100.0 * LAND_DETECTOR_VEL_Z_MAX * land_detector_scalar; // 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);