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.

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);
+
+			}
+		}
 	}
 }
 

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