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.

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;
 		}
 	}
 

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 */