From 33ba04112013035171865ce885d4777b0eca1ac7 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 22 May 2018 07:55:19 +0100
Subject: [PATCH] FlightTaskOrbit: improve yaw tracking with feed-forward

To make the vehicle front always point exactly to the center and not
lag behind.
---
 src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
index 0c0426b175..da401d5630 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
@@ -133,5 +133,8 @@ bool FlightTaskOrbit::update()
 
 	// make vehicle front always point towards the center
 	_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+	// yawspeed feed-forward because we know the necessary angular rate
+	_yawspeed_setpoint = -_v / _r;
+
 	return true;
 }