From b40dbd3d6f646f74fa2cec76c79be2bcda09f222 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 18 Aug 2020 15:47:31 +0200
Subject: [PATCH] FlightTaskAuto: hotfix filter for yawspeed feed-forward

to get rid of derivative spikes when navigator is
continuously updating the yaw setpoint in the
triplet for a POI but is running at a lower rate.

The proper solution is to generate that yaw setpoint
with high rate in the flight task and have the triplet
just guid to the next waypoint at low rate.
---
 .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp       | 6 ++++--
 .../flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp       | 2 ++
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
index edec9722ac..a2548be0ce 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -109,8 +109,10 @@ void FlightTaskAuto::_limitYawRate()
 		_yaw_setpoint = yaw_setpoint_sat;
 
 		if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
-			// Create a feedforward
-			_yawspeed_setpoint = dyaw / _deltatime;
+			// Create a feedforward using the filtered derivative
+			_yawspeed_filter.setParameters(_deltatime, .2f);
+			_yawspeed_filter.update(dyaw);
+			_yawspeed_setpoint = _yawspeed_filter.getState() / _deltatime;
 		}
 	}
 
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
index 55b8aea4d5..69180e8c59 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
@@ -46,6 +46,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <lib/geo/geo.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
 // TODO: make this switchable in the board config, like a module
 #if CONSTRAINED_FLASH
@@ -114,6 +115,7 @@ protected:
 	int _mission_gear{landing_gear_s::GEAR_KEEP};
 
 	float _yaw_sp_prev{NAN};
+	AlphaFilter<float> _yawspeed_filter;
 	bool _yaw_sp_aligned{false};
 
 	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */