From cd2b5e7479902eda91b1a7327f1314e28ad5ad12 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 20 Nov 2019 10:30:25 +0100
Subject: [PATCH] FlightTaskManualAltitude: Use dedicated member variable for
 yawspeed filter state insted of using the previous setpoint

---
 .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp        | 5 +++--
 .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp        | 2 ++
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index d5e653dfa6..b80eb72b6f 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -98,7 +98,8 @@ float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
 {
 	const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
 	const float alpha = _deltatime / den;
-	return (1.f - alpha) * _yawspeed_setpoint + alpha * yawspeed_target;
+	_yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target;
+	return _yawspeed_filter_state;
 }
 
 void FlightTaskManualAltitude::_updateAltitudeLock()
@@ -327,7 +328,7 @@ void FlightTaskManualAltitude::_lockYaw()
 	// hold the current heading when no more rotation commanded
 	if (!PX4_ISFINITE(_yaw_setpoint)) {
 		_yaw_setpoint = _yaw;
-		_yawspeed_setpoint = 0.f;
+		_yawspeed_setpoint = NAN;
 	}
 }
 
diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index 4b25bda933..edcdac545b 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -131,4 +131,6 @@ private:
 	 * _dist_to_ground_lock.
 	 */
 	float _dist_to_ground_lock = NAN;
+
+	float _yawspeed_filter_state{};
 };