From 8da1d3b16edd92aeb62bd93309e6830593b702c0 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 26 Feb 2018 13:34:07 +0100
Subject: [PATCH] FlightTasks: give every FlightTask access to prepared stick
 input and position state

---
 src/lib/FlightTasks/FlightTasks.hpp           |  4 +-
 src/lib/FlightTasks/tasks/FlightTask.hpp      | 49 ++++++++++++++++---
 src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 10 ++--
 3 files changed, 52 insertions(+), 11 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 2f4be76355..122df8e990 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -52,10 +52,9 @@ public:
 
 	/**
 	 * Call regularly in the control loop cycle to execute the task
-	 * @param TODO
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	int update() { return Orbit.update(); };
+	int update() { return _tasks[_current_task]->update(); };
 
 	void set_input_pointers(vehicle_local_position_s *vehicle_local_position,
 				manual_control_setpoint_s *manual_control_setpoint)
@@ -74,6 +73,7 @@ public:
 
 private:
 	static const int _task_count = 1;
+	int _current_task = 0;
 
 	FlightTaskOrbit Orbit;
 	FlightTask *_tasks[_task_count] = {&Orbit};
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 47d8454faf..63c36cfd9b 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -75,7 +75,11 @@ public:
 	 */
 	virtual int update()
 	{
-		_time = hrt_elapsed_time(&_starting_time_stamp) / 1e6;
+		_time = hrt_elapsed_time(&_starting_time_stamp) / 1e6f;
+		_deltatime  = math::min(hrt_elapsed_time(&_last_time_stamp) / 1e6f, (float)_timeout);
+		_last_time_stamp = hrt_absolute_time();
+		_evaluate_sticks();
+		_evaluate_position();
 		return 0;
 	};
 
@@ -99,9 +103,13 @@ public:
 
 protected:
 
-	float _get_time() { return _time; }
+	float _time = 0; /*< passed time in seconds since the task was activated */
+	float _deltatime = 0; /*< passed time in seconds since the task was last updated */
 	void _reset_time() { _starting_time_stamp = hrt_absolute_time(); };
 
+	matrix::Vector<float, 4> _sticks;
+	matrix::Vector3f _position;
+
 	void _set_position_setpoint(const matrix::Vector3f position_setpoint)
 	{
 		_vehicle_position_setpoint.x = position_setpoint(0);
@@ -124,16 +132,45 @@ protected:
 	};
 
 private:
+	static const int _timeout = 500000;
 
-	/* local time for a task */
-	float _time = 0; /*< passed time in seconds since the task was activated */
 	hrt_abstime _starting_time_stamp; /*< time stamp when task was activated */
+	hrt_abstime _last_time_stamp; /*< time stamp when task was last updated */
 
-	/* General Input */
+	/* General input that every task has */
 	const vehicle_local_position_s *_vehicle_local_position;
 	const manual_control_setpoint_s *_manual_control_setpoint;
 
-	/* General Output */
+	/* General output that every task has */
 	vehicle_local_position_setpoint_s _vehicle_position_setpoint;
 
+	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;
+
+		} else {
+			for (int i = 0; i < 3; i++) {
+				_position(i) = 0.f;
+			}
+		}
+	}
+
+	void _evaluate_sticks()
+	{
+		if (_manual_control_setpoint != NULL && hrt_elapsed_time(&_manual_control_setpoint->timestamp) < _timeout) {
+			_sticks(0) = _manual_control_setpoint->y; /* "roll" [-1,1] */
+			_sticks(1) = _manual_control_setpoint->x; /* "pitch" [-1,1] */
+			_sticks(2) = _manual_control_setpoint->r; /* "yaw" [-1,1] */
+			_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;
+			}
+		}
+	}
+
 };
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
index d683799bea..897cac0f7f 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
@@ -67,19 +67,23 @@ public:
 
 	/**
 	 * Call regularly in the control loop cycle to execute the task
-	 * @param TODO
 	 * @return 0 on success, >0 on error otherwise
 	 */
 	virtual int update()
 	{
 		FlightTask::update();
-		float v = 2 * M_PI_F * 0.1f; /* velocity for orbiting in radians per second */
+		r += _sticks(1) * _deltatime;
+		vu += _sticks(0) * _deltatime;
+		printf("%f %f %f\n", (double)_deltatime, (double)r, (double)vu);
+		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(1.f * cosf(v * _get_time()), 1.f * sinf(v * _get_time()), -altitude));
+		_set_position_setpoint(matrix::Vector3f(r * cosf(v * _time), r * sinf(v * _time), -altitude));
 		return 0;
 	};
 
 private:
 
+	float r = 2.f; /* radius with which to orbit the target */
+	float vu =  0.1f; /* velocity for orbiting in revolutions per second */
 
 };