From 585b03898fa5e7bd9714dda68a7922ba248cc1bf Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 9 Nov 2017 16:31:35 -0500
Subject: [PATCH] vtol_att_control: ack transition commands

Previously transition commands were not acked at all which meant that a
mavlink consumer such as a ground station would not get feedback if the
command arrived.

This solution is not optimal because it does not take into account if
the transition actually happened but at least it is feedback that the
command has arrived at the destination.
---
 .../vtol_att_control_main.cpp                 | 28 ++++++++++++++++++-
 .../vtol_att_control/vtol_att_control_main.h  |  2 ++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 4e65aec2c1..b1b5fded3a 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 - 2017 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -88,6 +88,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_vtol_vehicle_status_pub(nullptr),
 	_v_rates_sp_pub(nullptr),
 	_v_att_sp_pub(nullptr),
+	_v_cmd_ack_pub(nullptr),
 	_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
 	_abort_front_transition(false)
 
@@ -483,6 +484,31 @@ VtolAttitudeControl::handle_command()
 	// update transition command if necessary
 	if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
 		_transition_command = int(_vehicle_cmd.param1 + 0.5f);
+
+		// Report that we have received the command no matter what we actually do with it.
+		// This might not be optimal but is better than no response at all.
+
+		if (_vehicle_cmd.from_external) {
+			vehicle_command_ack_s command_ack = {
+				.timestamp = hrt_absolute_time(),
+				.result_param2 = 0,
+				.command = _vehicle_cmd.command,
+				.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
+				.from_external = false,
+				.result_param1 = 0,
+				.target_system = _vehicle_cmd.source_system,
+				.target_component = _vehicle_cmd.source_component
+			};
+
+			if (_v_cmd_ack_pub == nullptr) {
+				_v_cmd_ack_pub = 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), _v_cmd_ack_pub, &command_ack);
+
+			}
+		}
 	}
 }
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 8a34aca282..80c16eeb02 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -72,6 +72,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -159,6 +160,7 @@ private:
 	orb_advert_t	_vtol_vehicle_status_pub;
 	orb_advert_t	_v_rates_sp_pub;
 	orb_advert_t	_v_att_sp_pub;
+	orb_advert_t	_v_cmd_ack_pub;
 
 //*******************data containers***********************************************************
 	struct vehicle_attitude_s			_v_att;				//vehicle attitude