Copter: add filter for the land detector

Individual access filtering will be replaced with a single 3-axis low pass
filter in the near future

ArduCopter/ArduCopter.pde

@@ -534,10 +534,11 @@ static AP_Frsky_Telem frsky_telemetry(ahrs, battery);
 static int16_t climb_rate;
 // The altitude as reported by Sonar in cm - Values are 20 to 700 generally.
 static int16_t sonar_alt;
-static uint8_t sonar_alt_health;   // true if we can trust the altitude from the sonar
+static uint8_t sonar_alt_health;    // true if we can trust the altitude from the sonar
 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
 
 
 ////////////////////////////////////////////////////////////////////////////////

ArduCopter/land_detector.pde

@@ -26,8 +26,16 @@ static void update_land_detector()
     Vector3f accel_ef = ahrs.get_accel_ef_blended();
     accel_ef.z += GRAVITY_MSS;
 
+    // lowpass filter on accel
+    float freq_cut = 1.0f;
+    float dt = 0.02f; //This should be set from somewhere
+    float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*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 = (accel_ef.length() < 1.0f);
+    bool accel_stationary = (land_filtered_accel_ef.length() < 1.0f);
 
     if ( motor_at_lower_limit && accel_stationary) {
         if (!ap.land_complete) {