From 7a401e5ca3c87b214b04aa95b474337706e6564e Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 25 May 2020 15:19:04 +0200
Subject: [PATCH] FlightTaskAuto: Create artificial yaw speed sp for
 feedforward

---
 src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
index 7996e722b0..f33877d2c5 100644
--- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
@@ -110,6 +110,11 @@ void FlightTaskAuto::_limitYawRate()
 
 		_yaw_setpoint = yaw_setpoint_sat;
 		_yaw_sp_prev = _yaw_setpoint;
+
+		if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
+			// Create a feedforward
+			_yawspeed_setpoint = dyaw / _deltatime;
+		}
 	}
 
 	if (PX4_ISFINITE(_yawspeed_setpoint)) {