From c21c36dd8db2170b0c8341dce3860cfff43f9cc7 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Fri, 19 May 2017 18:44:46 +0200
Subject: [PATCH] FlightTasks: added wrappers for filling setpoint vectors,
 made time dependent example

---
 src/lib/FlightTasks/FlightTasks.hpp           |  4 +--
 src/lib/FlightTasks/tasks/FlightTask.hpp      | 34 ++++++++++++++-----
 src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 14 ++++++--
 3 files changed, 39 insertions(+), 13 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 5268599409..0fa21c749a 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -55,13 +55,13 @@ public:
 	 * @param TODO
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_local_position) { return 0; };
+	int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_local_position) { return Orbit.update(NULL, NULL); };
 
 	/**
 	 * Call to get result of the task execution
 	 * @return pointer to
 	 */
-	const vehicle_local_position_setpoint_s *get_local_position_setpoint() const { return Orbit.get_local_position_setpoint(); };
+	const vehicle_local_position_setpoint_s *get_position_setpoint() const { return Orbit.get_position_setpoint(); };
 
 private:
 
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 04adab2e85..7877de685d 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -47,12 +47,7 @@
 class FlightTask
 {
 public:
-	FlightTask()
-	{
-		vehicle_local_position_setpoint.x = 1;
-		vehicle_local_position_setpoint.y = 2;
-		vehicle_local_position_setpoint.z = -3;
-	};
+	FlightTask() {};
 	virtual ~FlightTask() {};
 
 	/**
@@ -79,10 +74,33 @@ public:
 	 * Call to get result of the task execution
 	 * @return pointer to
 	 */
-	const vehicle_local_position_setpoint_s *get_local_position_setpoint() const { return &vehicle_local_position_setpoint; };
+	const vehicle_local_position_setpoint_s *get_position_setpoint() const { return &_vehicle_position_setpoint; };
+
+protected:
+
+	void _set_position_setpoint(const matrix::Vector3f position_setpoint)
+	{
+		_vehicle_position_setpoint.x = position_setpoint(0);
+		_vehicle_position_setpoint.y = position_setpoint(1);
+		_vehicle_position_setpoint.z = position_setpoint(2);
+	};
+
+	void _set_velocity_setpoint(const matrix::Vector3f velocity_setpoint)
+	{
+		_vehicle_position_setpoint.vx = velocity_setpoint(0);
+		_vehicle_position_setpoint.vy = velocity_setpoint(1);
+		_vehicle_position_setpoint.vz = velocity_setpoint(2);
+	};
+
+	void _set_acceleration_setpoint(const matrix::Vector3f acceleration_setpoint)
+	{
+		_vehicle_position_setpoint.acc_x = acceleration_setpoint(0);
+		_vehicle_position_setpoint.acc_y = acceleration_setpoint(1);
+		_vehicle_position_setpoint.acc_z = acceleration_setpoint(2);
+	};
 
 private:
 
-	vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+	vehicle_local_position_setpoint_s _vehicle_position_setpoint;
 
 };
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
index df7b86ddae..4f160f1187 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
@@ -53,7 +53,11 @@ public:
 	 * Call once on the event where you switch to the task
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	virtual int activate() { return 0; };
+	virtual int activate()
+	{
+		_set_position_setpoint(matrix::Vector3f());
+		return 0;
+	};
 
 	/**
 	 * Call once on the event of switching away from the task
@@ -66,8 +70,12 @@ public:
 	 * @param TODO
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	virtual int update(manual_control_setpoint_s *manual_control_setpoint,
-			   vehicle_local_position_s *vehicle_local_position) { return 0; };
+	virtual int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_position)
+	{
+		float time = hrt_absolute_time() / 1e6;
+		_set_position_setpoint(matrix::Vector3f(0.1f * time, 0.f, -2.f));
+		return 0;
+	};
 
 private: