From b856c1048dbb2119aa185afd6365c2a0295ec170 Mon Sep 17 00:00:00 2001
From: Martina <martina@rivizzigno.it>
Date: Mon, 9 Jul 2018 11:53:35 +0200
Subject: [PATCH] FlightTask: add methods to get the desired trajectory
 waypoints

---
 src/lib/FlightTasks/FlightTasks.cpp               | 11 +++++++++++
 src/lib/FlightTasks/FlightTasks.hpp               |  6 ++++++
 .../FlightTasks/tasks/FlightTask/FlightTask.cpp   | 11 +++++++++++
 .../FlightTasks/tasks/FlightTask/FlightTask.hpp   | 15 +++++++++++++++
 4 files changed, 43 insertions(+)

diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index 2fe0b7063a..9e5cb76b62 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -47,6 +47,17 @@ const vehicle_constraints_s FlightTasks::getConstraints()
 	}
 }
 
+const vehicle_trajectory_waypoint_s FlightTasks::getAvoidanceWaypoint()
+{
+
+	if (isAnyTaskActive()) {
+		return _current_task.task->getAvoidanceWaypoint();
+
+	} else {
+		return FlightTask::empty_trajectory_waypoint;
+	}
+}
+
 int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 {
 	// switch to the running task, nothing to do
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 235a6820e4..fc30c8d7eb 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -77,6 +77,12 @@ public:
 	 */
 	const vehicle_constraints_s getConstraints();
 
+	/**
+	 * Get task avoidance desired waypoints
+	 * @return auto triplets in the mc_pos_control
+	 */
+	const vehicle_trajectory_waypoint_s getAvoidanceWaypoint();
+
 	/**
 	 * Switch to the next task in the available list (for testing)
 	 * @return 1 on success, <0 on error
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
index d62e9c4aa2..ebfc0520a6 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
@@ -5,7 +5,17 @@
 constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
 const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
+
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, vehicle_constraints_s::GEAR_KEEP, {}};
+const vehicle_trajectory_waypoint_s FlightTask::empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
+	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false}
+	}
+};
+
 bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
 	if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
@@ -69,6 +79,7 @@ void FlightTask::_resetSetpoints()
 	_acceleration_setpoint *= NAN;
 	_thrust_setpoint *= NAN;
 	_yaw_setpoint = _yawspeed_setpoint = NAN;
+	_desired_waypoint = FlightTask::empty_trajectory_waypoint;
 }
 
 bool FlightTask::_evaluateVehicleLocalPosition()
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index 9ac6e1ce7f..7f5fccd7e6 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -50,6 +50,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include "SubscriptionArray.hpp"
 
 class FlightTask : public ModuleParams
@@ -107,6 +108,8 @@ public:
 	 */
 	const vehicle_constraints_s getConstraints() {return _constraints;};
 
+	const vehicle_trajectory_waypoint_s getAvoidanceWaypoint() {return _desired_waypoint;};
+
 	/**
 	 * Empty setpoint.
 	 * All setpoints are set to NAN.
@@ -119,6 +122,12 @@ public:
 	 */
 	static const vehicle_constraints_s empty_constraints;
 
+	/**
+	 * Empty desired waypoints.
+	 * All waypoints are set to NAN.
+	 */
+	static const vehicle_trajectory_waypoint_s empty_trajectory_waypoint;
+
 	/**
 	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
 	 */
@@ -183,6 +192,12 @@ protected:
 	 */
 	vehicle_constraints_s _constraints;
 
+	/**
+	 * Desired waypoints.
+	 * Goals set by the FCU to be sent to the obstacle avoidance system.
+	 */
+	vehicle_trajectory_waypoint_s _desired_waypoint;
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
 					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,
 					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN,