From 17d4fd064fc06638d71a2cb9b34ee73edc71bccf Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 14 May 2020 17:11:43 +0200
Subject: [PATCH] FlightTaskManualAltitude: add an optional stick tilt input
 filter

---
 .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp       | 4 ++++
 .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp       | 6 +++++-
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 85be3474e4..c992e8cd5a 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -341,6 +341,10 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	// setpoint along z-direction, which is computed in PositionControl.cpp.
 
 	Vector2f sp(&_sticks(0));
+
+	_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
+	_man_input_filter.update(sp);
+	sp = _man_input_filter.getState();
 	_rotateIntoHeadingFrame(sp);
 
 	if (sp.length() > 1.0f) {
diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index efd47b5030..e333a987c7 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -40,6 +40,7 @@
 #pragma once
 
 #include "FlightTaskManual.hpp"
+#include <lib/ecl/EKF/AlphaFilter.hpp>
 
 class FlightTaskManualAltitude : public FlightTaskManual
 {
@@ -83,7 +84,8 @@ protected:
 					(ParamFloat<px4::params::MPC_LAND_SPEED>)
 					_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
 					(ParamFloat<px4::params::MPC_TKO_SPEED>)
-					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
+					_param_mpc_tko_speed, /**< desired upwards speed when still close to the ground */
+					(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
 				       )
 private:
 	bool _isYawInput();
@@ -137,4 +139,6 @@ private:
 	 * _dist_to_ground_lock.
 	 */
 	float _dist_to_ground_lock = NAN;
+
+	AlphaFilter<matrix::Vector2f> _man_input_filter;
 };