diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 63c36cfd9b..d84b77d8d9 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -80,6 +80,7 @@ public:
 		_last_time_stamp = hrt_absolute_time();
 		_evaluate_sticks();
 		_evaluate_position();
+		_evaluate_velocity();
 		return 0;
 	};
 
@@ -107,8 +108,10 @@ protected:
 	float _deltatime = 0; /*< passed time in seconds since the task was last updated */
 	void _reset_time() { _starting_time_stamp = hrt_absolute_time(); };
 
+	/* Prepared general inputs for every task */
 	matrix::Vector<float, 4> _sticks;
 	matrix::Vector3f _position;
+	matrix::Vector3f _velocity;
 
 	void _set_position_setpoint(const matrix::Vector3f position_setpoint)
 	{
@@ -147,14 +150,17 @@ private:
 	void _evaluate_position()
 	{
 		if (_vehicle_local_position != NULL && hrt_elapsed_time(&_vehicle_local_position->timestamp) < _timeout) {
-			_position(0) = _vehicle_local_position->x;
-			_position(1) = _vehicle_local_position->y;
-			_position(2) = _vehicle_local_position->z;
+			_position = matrix::Vector3f(&_vehicle_local_position->x);
+		}
+	}
+
+	void _evaluate_velocity()
+	{
+		if (_vehicle_local_position != NULL && hrt_elapsed_time(&_vehicle_local_position->timestamp) < _timeout) {
+			_velocity = matrix::Vector3f(&_vehicle_local_position->vx);
 
 		} else {
-			for (int i = 0; i < 3; i++) {
-				_position(i) = 0.f;
-			}
+			_velocity = matrix::Vector3f(); /* default is all zero */
 		}
 	}
 
@@ -167,9 +173,7 @@ private:
 			_sticks(3) = (_manual_control_setpoint->z - 0.5f) * 2.f; /* "thrust" resacaled from [0,1] to [-1,1] */
 
 		} else {
-			for (int i = 0; i < 4; i++) {
-				_sticks(i) = 0.f;
-			}
+			_sticks = matrix::Vector<float, 4>(); /* default is all zero */
 		}
 	}
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
index 897cac0f7f..6d77d34aac 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
@@ -73,10 +73,12 @@ public:
 	{
 		FlightTask::update();
 		r += _sticks(1) * _deltatime;
-		vu += _sticks(0) * _deltatime;
-		printf("%f %f %f\n", (double)_deltatime, (double)r, (double)vu);
+		vu += _sticks(0) * 0.1f * _deltatime;
+		altitude += _sticks(3) * _deltatime;
+		//printf("%f %f %f\n", (double)altitude, (double)r, (double)vu);
+		//printf("%f %f %f\n", (double)_position(0), (double)_position(1), (double)_position(2));
+		printf("%f %f %f\n", (double)_velocity(0), (double)_velocity(1), (double)_velocity(2));
 		float v = 2 * M_PI_F * vu; /* velocity for orbiting in radians per second */
-		float altitude = 2; /* altitude in meters */
 		_set_position_setpoint(matrix::Vector3f(r * cosf(v * _time), r * sinf(v * _time), -altitude));
 		return 0;
 	};
@@ -85,5 +87,6 @@ private:
 
 	float r = 2.f; /* radius with which to orbit the target */
 	float vu =  0.1f; /* velocity for orbiting in revolutions per second */
+	float altitude = 2; /* altitude in meters */
 
 };