diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index b295c4f37a..ef6ba69b50 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -184,63 +184,67 @@ const char *FlightTasks::errorToString(const int error)
 
 void FlightTasks::_updateCommand()
 {
-//	TODO: port flight task mavlink command to have support for this functionality
-//	/* 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;
-//	}
-//
-//	/* 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;
-//	}
-//
-//	/* evaluate command */
-//	// TODO: remapping of the Mavlink definition to the internal tasks necessary
-//	int switch_result = switchTask(int(command.param1 + 0.5f));
-//	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
-//
-//	/* if we are in the desired task */
-//	if (switch_result >= 0) {
-//		cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
-//
-//		/* if the task is running apply parameters to it and see if it rejects */
-//		if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
-//			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
-//
-//			/* if we just switched and parameters are not accepted, go to failsafe */
-//			if (switch_result == 1) {
-//				switchTask(-1);
-//				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
-//			}
-//		}
-//	}
-//
-//	/* 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);
-//
-//	}
+	// 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;
+	}
+
+	// get command
+	struct vehicle_command_s command;
+	orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
+
+	// check what command it is
+	FlightTaskIndex desired_task = FlightTaskIndex::None;
+	switch(command.command) {
+		case vehicle_command_s::VEHICLE_CMD_DO_ORBIT :
+			desired_task = FlightTaskIndex::Orbit;
+			break;
+		// ignore all unkown commands
+		default : return;
+	}
+
+	// switch to the commanded task
+	int switch_result = switchTask(desired_task);
+	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+
+	// if we are in/switched to the desired task
+	if (switch_result >= 0) {
+		cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+		// if the task is running apply parameters to it and see if it rejects
+		if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
+			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+
+			// if we just switched and parameters are not accepted, go to failsafe
+			if (switch_result == 1) {
+				switchTask(FlightTaskIndex::Position);
+				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+			}
+		}
+	}
+
+	// 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);
+
+	}
 }
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index c7d57a7367..6cea623953 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -188,8 +188,8 @@ private:
 	 * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
 	 */
 	void _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 */
 
 	int _initTask(FlightTaskIndex task_index);
-//	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 */
 };