FlightTaskAuto: Create artificial yaw speed sp for feedforward

5 years ago · 7a401e5ca3
1 changed files with 5 additions and 0 deletions
--- 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()
				@@ -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)) {