From 29ca7a10df494ec10b69def504c00cd3facb78c9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 27 May 2014 22:44:57 +0900
Subject: [PATCH] AC_PosControl: set alt hold accel control D term filter

---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 10 ++++++++++
 libraries/AC_AttitudeControl/AC_PosControl.h   |  5 ++++-
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index e5653cecfa..606d61c0dd 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -73,6 +73,16 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
 /// z-axis position controller
 ///
 
+
+/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
+void AC_PosControl::set_dt(float delta_sec)
+{
+    _dt = delta_sec;
+
+    // update rate controller's d filter
+    _pid_alt_accel.set_d_lpf_alpha(POSCONTROOL_ACCEL_Z_DTERM_FILTER, _dt);
+}
+
 /// set_speed_z - sets maximum climb and descent rates
 /// To-Do: call this in the main code as part of flight mode initialisation
 ///     calc_leash_length_z should be called afterwards
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index fbdd35cc2c..6823be3da5 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -36,6 +36,8 @@
 
 #define POSCONTROL_ACTIVE_TIMEOUT_MS            200     // position controller is considered active if it has been called within the past 200ms (0.2 seconds)
 
+#define POSCONTROOL_ACCEL_Z_DTERM_FILTER        20      // Z axis accel controller's D term filter (in hz)
+
 class AC_PosControl
 {
 public:
@@ -51,7 +53,8 @@ public:
     ///
 
     /// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
-    void set_dt(float delta_sec) { _dt = delta_sec; }
+    ///     updates z axis accel controller's D term filter
+    void set_dt(float delta_sec);
     float get_dt() const { return _dt; }
 
     ///