Copter: use LowPassFilterVector3f for land detector accel filter

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

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

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
+    accel_ef = land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
-    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);
 
     // 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) {