From db0ba763bffa7affcd8a8a4f9177ff17213cb5c3 Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Sat, 8 Nov 2014 13:32:40 +0100
Subject: [PATCH] OP-1599 add low pass filter to control input in vario mode to
 possibly reduce rubber-band effect - needs test and tuning

---
 flight/libraries/plans.c                      | 19 ++++++++++++++++++-
 .../flightmodesettings.xml                    |  1 +
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c
index 3bab3efaa..179e371e8 100644
--- a/flight/libraries/plans.c
+++ b/flight/libraries/plans.c
@@ -167,11 +167,16 @@ void plan_run_land()
  */
 static bool vario_hold    = true;
 static float hold_position[3];
-static float vario_course = 0;
+static float vario_control_lowpass[4];
+static float vario_course = 0.0f;
 
 static void plan_setup_PositionVario()
 {
     vario_hold = true;
+    vario_control_lowpass[0] = 0.0f;
+    vario_control_lowpass[1] = 0.0f;
+    vario_control_lowpass[2] = 0.0f;
+    vario_control_lowpass[3] = 0.0f;
     AttitudeStateYawGet(&vario_course);
     plan_setup_positionHold();
 }
@@ -293,6 +298,7 @@ static void getVector(float controlVector[4], vario_type type)
 static void plan_run_PositionVario(vario_type type)
 {
     float controlVector[4];
+    float alpha;
     PathDesiredData pathDesired;
 
     PathDesiredGet(&pathDesired);
@@ -305,6 +311,17 @@ static void plan_run_PositionVario(vario_type type)
     ManualControlCommandYawGet(&controlVector[2]);
     ManualControlCommandThrustGet(&controlVector[3]);
 
+
+    FlightModeSettingsVarioControlLowPassAlphaGet(&alpha);
+    vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0];
+    vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1];
+    vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2];
+    vario_control_lowpass[3] = alpha * vario_control_lowpass[3] + (1.0f - alpha) * controlVector[3];
+    controlVector[0] = vario_control_lowpass[0];
+    controlVector[1] = vario_control_lowpass[1];
+    controlVector[2] = vario_control_lowpass[2];
+    controlVector[3] = vario_control_lowpass[3];
+
     // check if movement is desired
     if (normalizeDeadband(controlVector) == false) {
         // no movement desired, re-enter positionHold at current start-position
diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml
index 97698afcb..25b2c57dc 100644
--- a/shared/uavobjectdefinition/flightmodesettings.xml
+++ b/shared/uavobjectdefinition/flightmodesettings.xml
@@ -114,6 +114,7 @@
         <field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="10,2"/>
 	<!-- optimized for current vtolpathfollower,
 	     for fixed wing pathfollower set to Horizontal=500,Vertical=5 -->
+	<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.9"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>