diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 3eeda6e3d0..ac8a3be2da 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -538,8 +538,7 @@ static uint8_t sonar_alt_health; // true if we can trust the altitude from th static float target_sonar_alt; // desired altitude in cm above the ground static int32_t baro_alt; // barometer altitude in cm above home static float baro_climbrate; // barometer climbrate in cm/s -Vector3f land_filtered_accel_ef; // accelerations for land detector test - +static LowPassFilterVector3f land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF); // accelerations for land detector test //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5e913a3fca..4ca41a1842 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -379,6 +379,9 @@ #ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC # define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of 50hz iterations with near zero climb rate and low throttle that means we might be landed (used to reset horizontal position targets to prevent tipping over) #endif +#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF +# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter +#endif #ifndef LAND_DETECTOR_CLIMBRATE_MAX # define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s #endif diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index f46bd610e8..10960e1501 100644 --- a/ArduCopter/land_detector.pde +++ b/ArduCopter/land_detector.pde @@ -27,16 +27,10 @@ static void update_land_detector() accel_ef.z += GRAVITY_MSS; // lowpass filter on accel - // TODO write and use LowPassFilterVector3f - const static float freq_cut = 1.0f; - const static float dt = 1.0f/MAIN_LOOP_RATE; - const static float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI_F*freq_cut)),0.0f,1.0f); - land_filtered_accel_ef.x += alpha * (accel_ef.x - land_filtered_accel_ef.x); - land_filtered_accel_ef.y += alpha * (accel_ef.y - land_filtered_accel_ef.y); - land_filtered_accel_ef.z += alpha * (accel_ef.z - land_filtered_accel_ef.z); + accel_ef = land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS); // check that the airframe is not accelerating (not falling or breaking after fast forward flight) - bool accel_stationary = (land_filtered_accel_ef.length() < 1.0f); + bool accel_stationary = (accel_ef.length() < 1.0f); if ( motor_at_lower_limit && accel_stationary) { if (!ap.land_complete) {