From 59a26678704bb25a9a0e1b7aaffa54437d85befc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 4 Mar 2020 15:05:13 +0900
Subject: [PATCH] Plane: quadplane's throttle mix uses filtered accelerations

---
 ArduPlane/quadplane.cpp | 9 ++++++---
 ArduPlane/quadplane.h   | 3 +++
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 189fa36a58..fff644b879 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -3182,6 +3182,11 @@ float QuadPlane::stopping_distance(void)
 
 void QuadPlane::update_throttle_mix(void)
 {
+    // update filtered acceleration
+    Vector3f accel_ef = ahrs.get_accel_ef_blended();
+    accel_ef.z += GRAVITY_MSS;
+    throttle_mix_accel_ef_filter.apply(accel_ef, plane.scheduler.get_loop_period_s());
+
     // transition will directly manage the mix
     if (in_transition()) {
       return;
@@ -3212,9 +3217,7 @@ void QuadPlane::update_throttle_mix(void)
         bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
 
         // check for large acceleration - falling or high turbulence
-        Vector3f accel_ef = plane.ahrs.get_accel_ef_blended();
-        accel_ef.z += GRAVITY_MSS;
-        bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);
+        bool accel_moving = (throttle_mix_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING);
 
         // check for requested decent
         bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 63baaba0ad..006ed365cf 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -389,6 +389,9 @@ private:
         float vpos_start_m;
     } landing_detect;
 
+    // throttle mix acceleration filter
+    LowPassFilterVector3f throttle_mix_accel_ef_filter = LowPassFilterVector3f(1.0f);
+
     // time we last set the loiter target
     uint32_t last_loiter_ms;