From b1f24da05e3850176670e46eac1b11241b2f031b Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 5 Dec 2017 18:10:47 +0100
Subject: [PATCH] FlightTasks: enable mavlink command handling for switching
 tasks including acknowledgement

---
 src/lib/FlightTasks/CMakeLists.txt  |  1 +
 src/lib/FlightTasks/FlightTasks.cpp | 57 +++++++++++++++++++++++++++++
 src/lib/FlightTasks/FlightTasks.hpp | 26 ++++++++-----
 3 files changed, 74 insertions(+), 10 deletions(-)
 create mode 100644 src/lib/FlightTasks/FlightTasks.cpp

diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt
index 0164d11a5c..1aabf393f3 100644
--- a/src/lib/FlightTasks/CMakeLists.txt
+++ b/src/lib/FlightTasks/CMakeLists.txt
@@ -37,6 +37,7 @@ px4_add_module(
 		tasks/FlightTaskManual.cpp
 		tasks/FlightTaskOrbit.cpp
 		SubscriptionArray.cpp
+		FlightTasks.cpp
 	DEPENDS
 		platforms__common
 		modules__uORB
diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
new file mode 100644
index 0000000000..3fd9f61023
--- /dev/null
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -0,0 +1,57 @@
+#include "FlightTasks.hpp"
+
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+bool FlightTasks::_updateCommand()
+{
+	/* lazy subscription to command topic */
+	if (_sub_vehicle_command < 0) {
+		_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+	}
+
+	/* check if there's any new command */
+	bool updated = false;
+	orb_check(_sub_vehicle_command, &updated);
+
+	if (!updated) {
+		return false;
+	}
+
+	/* check if command is for flight task library */
+	struct vehicle_command_s command;
+	orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
+
+	if (!(command.command == vehicle_command_s::VEHICLE_CMD_FLIGHT_TASK)) {
+		return false;
+	}
+
+	/* evaluate command */
+	//printf("YAY %d\n", int(command.param1));
+
+	uint8_t switch_result = switchTask(int(command.param1));
+	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+
+	if (!switch_result) {
+		cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+	}
+
+	/* send back acknowledgment */
+	vehicle_command_ack_s command_ack = {};
+	command_ack.command = command.command;
+	command_ack.result = cmd_result;
+	command_ack.result_param1 = switch_result;
+	command_ack.target_system = command.source_system;
+	command_ack.target_component = command.source_component;
+
+	if (_pub_vehicle_command_ack == nullptr) {
+		_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
+					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);
+
+	}
+
+	return true;
+}
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index c0eef19df3..f856975155 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -69,29 +69,27 @@ public:
 	 */
 	bool update()
 	{
+		_updateCommand();
+
 		if (isAnyTaskActive()) {
 			_subscription_array.update();
 			return _current_task->updateInitialize() && _current_task->update();
 		}
 
-		return 1;
+		return false;
 	}
 
 	/**
 	 * Get the output data from the current task
+	 * Only call when task is active!
 	 */
-	const vehicle_local_position_setpoint_s &getPositionSetpoint()
-	{
-		return _current_task->getPositionSetpoint();
-	}
+	const vehicle_local_position_setpoint_s &getPositionSetpoint() { return _current_task->getPositionSetpoint(); }
 
 	/**
-	 * Call this function initially to point all tasks to the general output data
+	 * Convenient operator to get the output data from the current task
+	 * Only call when task is active!
 	 */
-	inline const vehicle_local_position_setpoint_s &operator()()
-	{
-		return getPositionSetpoint();
-	}
+	inline const vehicle_local_position_setpoint_s &operator()() { return getPositionSetpoint(); }
 
 	/**
 	 * Switch to the next task in the available list (for testing)
@@ -187,4 +185,12 @@ private:
 	int _current_task_index = -1;
 
 	SubscriptionArray _subscription_array;
+
+	/**
+	 * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
+	 * @return true if there was a new command, false if not
+	 */
+	bool _updateCommand();
+	int	_sub_vehicle_command = -1; /*< topic handle on which commands are received */
+	orb_advert_t _pub_vehicle_command_ack = nullptr; /*< topic handle to which commands get acknowledged */
 };