From 12ea1d6e857ff4d7fa7ae05b62a36311fe69c80d Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Tue, 18 Nov 2014 05:54:33 +1100
Subject: [PATCH] AC_AttitudeControl: Scale angle demand noise filter

Scales filter frequency using EKF nav gain scaler to take advantage of the reduced nav frequency whichcan tolerate a slower angle response.
This is required to reduce the effect of EKF optical flow noise increase with height.
---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 8 ++++----
 libraries/AC_AttitudeControl/AC_PosControl.h   | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index ab78fc0e71..a1290fbcb1 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -571,7 +571,7 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity, float ekfNav
             break;
         case 3:
             // run position controller's acceleration to lean angle step
-            accel_to_lean_angles();
+            accel_to_lean_angles(ekfNavVelGainScaler);
             _xy_step++;
             break;
     }
@@ -642,7 +642,7 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
         rate_to_accel_xy(dt_xy, ekfNavVelGainScaler);
 
         // run acceleration to lean angle step
-        accel_to_lean_angles();
+        accel_to_lean_angles(ekfNavVelGainScaler);
     }
 
     // update altitude target
@@ -821,7 +821,7 @@ void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler)
 
 /// accel_to_lean_angles - horizontal desired acceleration to lean angles
 ///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
-void AC_PosControl::accel_to_lean_angles()
+void AC_PosControl::accel_to_lean_angles(float ekfNavVelGainScaler)
 {
     // catch if we've just been started
     uint32_t now = hal.scheduler->millis();
@@ -861,7 +861,7 @@ void AC_PosControl::accel_to_lean_angles()
     lastPitchDem = _pitch_target;
 
     // 5Hz lowpass filter on angles - required due to optical flow  noise
-    float freq_cut = 5.0f;
+    float freq_cut = 5.0f * ekfNavVelGainScaler;
     float alpha = constrain_float(dt_xy/(dt_xy + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f);
     static float roll_target_filtered = 0.0f;
     static float pitch_target_filtered = 0.0f;
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index 3e8a7df495..39fb76cdcc 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -317,7 +317,7 @@ private:
 
     /// accel_to_lean_angles - horizontal desired acceleration to lean angles
     ///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
-    void accel_to_lean_angles();
+    void accel_to_lean_angles(float ekfNavVelGainScaler);
 
     /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
     float calc_leash_length(float speed_cms, float accel_cms, float kP) const;