consume all available queued vehicle_commands

src/modules/commander/Commander.cpp

@@ -2415,7 +2415,7 @@ Commander::run()
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
-		if (_cmd_sub.updated()) {
+		while (_cmd_sub.updated()) {
 			/* got command */
 			const unsigned last_generation = _cmd_sub.get_last_generation();
 			vehicle_command_s cmd;

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -440,43 +440,44 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
 void FlightModeManager::handleCommand()
 {
 	// get command
-	vehicle_command_s command{};
-	_vehicle_command_sub.copy(&command);
+	vehicle_command_s command;
 
-	// check what command it is
-	FlightTaskIndex desired_task = switchVehicleCommand(command.command);
+	while (_vehicle_command_sub.update(&command)) {
+		// check what command it is
+		FlightTaskIndex desired_task = switchVehicleCommand(command.command);
 
-	// ignore all unkown commands
-	if (desired_task != FlightTaskIndex::None) {
-		// switch to the commanded task
-		FlightTaskError switch_result = switchTask(desired_task);
-		uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+		// ignore all unkown commands
+		if (desired_task != FlightTaskIndex::None) {
+			// switch to the commanded task
+			FlightTaskError 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 >= FlightTaskError::NoError) {
-			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+			// if we are in/switched to the desired task
+			if (switch_result >= FlightTaskError::NoError) {
+				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 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 >= FlightTaskError::NoError) {
-					switchTask(FlightTaskIndex::ManualPosition);
-					cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+					// if we just switched and parameters are not accepted, go to failsafe
+					if (switch_result >= FlightTaskError::NoError) {
+						switchTask(FlightTaskIndex::ManualPosition);
+						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 = static_cast<int>(switch_result);
-		command_ack.target_system = command.source_system;
-		command_ack.target_component = command.source_component;
-		command_ack.timestamp = hrt_absolute_time();
-		_vehicle_command_ack_pub.publish(command_ack);
+			// send back acknowledgment
+			vehicle_command_ack_s command_ack{};
+			command_ack.command = command.command;
+			command_ack.result = cmd_result;
+			command_ack.result_param1 = static_cast<int>(switch_result);
+			command_ack.target_system = command.source_system;
+			command_ack.target_component = command.source_component;
+			command_ack.timestamp = hrt_absolute_time();
+			_vehicle_command_ack_pub.publish(command_ack);
+		}
 	}
 }
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -162,9 +162,19 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 void
 FixedwingPositionControl::vehicle_command_poll()
 {
-	if (_vehicle_command_sub.updated()) {
-		_vehicle_command_sub.copy(&_vehicle_command);
-		handle_command();
+	vehicle_command_s vehicle_command;
+
+	while (_vehicle_command_sub.update(&vehicle_command)) {
+		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
+			// only abort landing before point of no return (horizontal and vertical)
+			if (_control_mode.flag_control_auto_enabled &&
+			    _pos_sp_triplet.current.valid &&
+			    (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
+
+				abort_landing(true);
+			}
+		}
+
 	}
 }
 
@@ -1597,20 +1607,6 @@ FixedwingPositionControl::get_tecs_thrust()
 	return 0.0f;
 }
 
-void
-FixedwingPositionControl::handle_command()
-{
-	if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
-		// only abort landing before point of no return (horizontal and vertical)
-		if (_control_mode.flag_control_auto_enabled &&
-		    _pos_sp_triplet.current.valid &&
-		    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-
-			abort_landing(true);
-		}
-	}
-}
-
 void
 FixedwingPositionControl::Run()
 {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -170,7 +170,6 @@ private:
 	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items
 	vehicle_attitude_s		_att {};			///< vehicle attitude setpoint
 	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint
-	vehicle_command_s		_vehicle_command {};		///< vehicle commands
 	vehicle_control_mode_s		_control_mode {};		///< control mode
 	vehicle_local_position_s	_local_pos {};			///< vehicle local position
 	vehicle_land_detected_s		_vehicle_land_detected {};	///< vehicle land detected
@@ -346,11 +345,6 @@ private:
 	float		get_demanded_airspeed();
 	float		calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed);
 
-	/**
-	 * Handle incoming vehicle commands
-	 */
-	void		handle_command();
-
 	void		reset_takeoff_state(bool force = false);
 	void		reset_landing_state();
 

src/modules/mavlink/mavlink_main.cpp

@@ -183,6 +183,13 @@ Mavlink::Mavlink() :
 	if (_first_start_time == 0) {
 		_first_start_time = hrt_absolute_time();
 	}
+
+	// ensure topic exists, otherwise we might lose first queued commands
+	if (!orb_exists(ORB_ID(vehicle_command), 0)) {
+		orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH);
+	}
+
+	_vehicle_command_sub.subscribe();
 }
 
 Mavlink::~Mavlink()
@@ -2135,19 +2142,6 @@ Mavlink::task_main(int argc, char *argv[])
 		pthread_mutex_init(&_message_buffer_mutex, nullptr);
 	}
 
-	uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
-	// ensure topic exists, otherwise we might lose first queued commands (leading to printf error's below)
-	orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH);
-	cmd_sub.subscribe();
-	uORB::Subscription status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription ack_sub{ORB_ID(vehicle_command_ack)};
-
-	/* command ack */
-	uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
-
-	vehicle_status_s status{};
-	status_sub.copy(&status);
-
 	/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
 	_transmitting_enabled = true;
 	_transmitting_enabled_commanded = true;
@@ -2263,81 +2257,84 @@ Mavlink::task_main(int argc, char *argv[])
 
 		configure_sik_radio();
 
-		if (status_sub.update(&status)) {
-			/* switch HIL mode if required */
-			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status;
 
-			set_generate_virtual_rc_input(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
+			if (_vehicle_status_sub.copy(&vehicle_status)) {
+				/* switch HIL mode if required */
+				set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
 
-			if (_mode == MAVLINK_MODE_IRIDIUM) {
+				set_generate_virtual_rc_input(vehicle_status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
 
-				if (_transmitting_enabled &&
-				    status.high_latency_data_link_lost &&
-				    !_transmitting_enabled_commanded &&
-				    (_first_heartbeat_sent)) {
+				if (_mode == MAVLINK_MODE_IRIDIUM) {
 
-					_transmitting_enabled = false;
-					mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
+					if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost &&
+					    !_transmitting_enabled_commanded && _first_heartbeat_sent) {
 
-				} else if (!_transmitting_enabled && !status.high_latency_data_link_lost) {
-					_transmitting_enabled = true;
-					mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
+						_transmitting_enabled = false;
+						mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
+
+					} else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) {
+						_transmitting_enabled = true;
+						mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
+					}
 				}
 			}
 		}
 
 
 		// vehicle_command
-		const unsigned last_generation = cmd_sub.get_last_generation();
-		vehicle_command_s vehicle_cmd;
+		while (_vehicle_command_sub.updated()) {
+			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+			vehicle_command_s vehicle_cmd;
 
-		if (cmd_sub.update(&vehicle_cmd)) {
-
-			if (cmd_sub.get_last_generation() != last_generation + 1) {
-				PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, cmd_sub.get_last_generation());
-			}
-
-			if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
-			    (_mode == MAVLINK_MODE_IRIDIUM)) {
-				if (vehicle_cmd.param1 > 0.5f) {
-					if (!_transmitting_enabled) {
-						mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
-								 _device_name);
-					}
-
-					_transmitting_enabled = true;
-					_transmitting_enabled_commanded = true;
-
-				} else {
-					if (_transmitting_enabled) {
-						mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
-								 _device_name);
-					}
-
-					_transmitting_enabled = false;
-					_transmitting_enabled_commanded = false;
+			if (_vehicle_command_sub.update(&vehicle_cmd)) {
+				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
+					PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation());
 				}
 
-				// send positive command ack
-				vehicle_command_ack_s command_ack{};
-				command_ack.timestamp = vehicle_cmd.timestamp;
-				command_ack.command = vehicle_cmd.command;
-				command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
-				command_ack.from_external = !vehicle_cmd.from_external;
-				command_ack.target_system = vehicle_cmd.source_system;
-				command_ack.target_component = vehicle_cmd.source_component;
+				if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
+				    (_mode == MAVLINK_MODE_IRIDIUM)) {
+					if (vehicle_cmd.param1 > 0.5f) {
+						if (!_transmitting_enabled) {
+							mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
+									 _device_name);
+						}
 
-				command_ack_pub.publish(command_ack);
+						_transmitting_enabled = true;
+						_transmitting_enabled_commanded = true;
+
+					} else {
+						if (_transmitting_enabled) {
+							mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
+									 _device_name);
+						}
+
+						_transmitting_enabled = false;
+						_transmitting_enabled_commanded = false;
+					}
+
+					// send positive command ack
+					vehicle_command_ack_s command_ack{};
+					command_ack.command = vehicle_cmd.command;
+					command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+					command_ack.from_external = !vehicle_cmd.from_external;
+					command_ack.target_system = vehicle_cmd.source_system;
+					command_ack.target_component = vehicle_cmd.source_component;
+					command_ack.timestamp = vehicle_cmd.timestamp;
+					_vehicle_command_ack_pub.publish(command_ack);
+				}
 			}
 		}
 
 		/* send command ACK */
-		uint16_t current_command_ack = 0;
+		bool cmd_logging_start_acknowledgement = false;
 
-		if (ack_sub.updated() && (get_free_tx_buf() >= MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
+		if (_vehicle_command_ack_sub.updated()) {
+			static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 			vehicle_command_ack_s command_ack;
 
-			if (ack_sub.update(&command_ack)) {
+			while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.update(&command_ack)) {
 				if (!command_ack.from_external) {
 					mavlink_command_ack_t msg;
 					msg.result = command_ack.result;
@@ -2346,13 +2343,16 @@ Mavlink::task_main(int argc, char *argv[])
 					msg.result_param2 = command_ack.result_param2;
 					msg.target_system = command_ack.target_system;
 					msg.target_component = command_ack.target_component;
-					current_command_ack = command_ack.command;
 
 					// TODO: always transmit the acknowledge once it is only sent over the instance the command is received
 					//bool _transmitting_enabled_temp = _transmitting_enabled;
 					//_transmitting_enabled = true;
 					mavlink_msg_command_ack_send_struct(get_channel(), &msg);
 					//_transmitting_enabled = _transmitting_enabled_temp;
+
+					if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
+						cmd_logging_start_acknowledgement = true;
+					}
 				}
 			}
 		}
@@ -2398,7 +2398,7 @@ Mavlink::task_main(int argc, char *argv[])
 				_mavlink_ulog_stop_requested = false;
 
 			} else {
-				if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
+				if (cmd_logging_start_acknowledgement) {
 					_mavlink_ulog->start_ack_received();
 				}
 
@@ -2612,7 +2612,7 @@ void Mavlink::publish_telemetry_status()
 
 	// telemetry_status is also updated from the receiver thread, but never the same fields
 	_tstatus.timestamp = hrt_absolute_time();
-	_telem_status_pub.publish(_tstatus);
+	_telemetry_status_pub.publish(_tstatus);
 	_tstatus_updated = false;
 }
 

src/modules/mavlink/mavlink_main.h

@@ -75,6 +75,9 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include "mavlink_command_sender.h"
 #include "mavlink_messages.h"
@@ -530,9 +533,13 @@ private:
 
 	orb_advert_t		_mavlink_log_pub{nullptr};
 
-	uORB::PublicationMulti<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
+	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationMulti<telemetry_status_s> _telemetry_status_pub{ORB_ID(telemetry_status)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	bool			_task_running{true};
 	static bool		_boot_complete;

src/modules/mavlink/mavlink_messages.cpp

@@ -506,7 +506,7 @@ public:
 	}
 
 private:
-	uORB::Subscription _cmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	/* do not allow top copying this class */
 	MavlinkStreamCommandLong(MavlinkStreamCommandLong &) = delete;
@@ -518,19 +518,28 @@ protected:
 
 	bool send() override
 	{
-		struct vehicle_command_s cmd;
 		bool sent = false;
 
-		if (_cmd_sub.update(&cmd)) {
+		while ((_mavlink->get_free_tx_buf() >= get_size()) && _vehicle_command_sub.updated()) {
 
-			if (!cmd.from_external) {
-				PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
+			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+			vehicle_command_s cmd;
 
-				MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
-				sent = true;
+			if (_vehicle_command_sub.update(&cmd)) {
+				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
+					PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
+						_vehicle_command_sub.get_last_generation());
+				}
 
-			} else {
-				PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
+				if (!cmd.from_external) {
+					PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
+
+					MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
+					sent = true;
+
+				} else {
+					PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
+				}
 			}
 		}
 

src/modules/navigator/navigator_main.cpp

@@ -212,7 +212,7 @@ Navigator::run()
 		_position_controller_status_sub.update();
 		_home_pos_sub.update(&_home_pos);
 
-		if (_vehicle_command_sub.updated()) {
+		while (_vehicle_command_sub.updated()) {
 			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
 			vehicle_command_s cmd{};
 			_vehicle_command_sub.copy(&cmd);

src/modules/temperature_compensation/TemperatureCompensationModule.cpp

@@ -181,7 +181,7 @@ void TemperatureCompensationModule::Run()
 	perf_begin(_loop_perf);
 
 	// Check if user has requested to run the calibration routine
-	if (_vehicle_command_sub.updated()) {
+	while (_vehicle_command_sub.updated()) {
 		vehicle_command_s cmd;
 
 		if (_vehicle_command_sub.copy(&cmd)) {

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -139,52 +139,40 @@ VtolAttitudeControl::init()
 	return true;
 }
 
-/**
-* Check for command updates.
-*/
-void
-VtolAttitudeControl::vehicle_cmd_poll()
+void VtolAttitudeControl::vehicle_cmd_poll()
 {
-	if (_vehicle_cmd_sub.updated()) {
-		_vehicle_cmd_sub.copy(&_vehicle_cmd);
-		handle_command();
-	}
-}
+	vehicle_command_s vehicle_command;
 
-/**
-* Check received command
-*/
-void
-VtolAttitudeControl::handle_command()
-{
-	if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
+	while (_vehicle_cmd_sub.update(&vehicle_command)) {
+		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
 
-		vehicle_status_s vehicle_status = {};
-		_vehicle_status_sub.copy(&vehicle_status);
+			uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
 
-		uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+			// deny any transition in auto takeoff mode, plus transition from RW to FW in land or RTL mode
+			if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
+			    || (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+				&& (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+				    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))) {
 
-		// deny any transition in auto takeoff mode, plus transition from RW to FW in land or RTL mode
-		if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
-		    || (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-			&& (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
-			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))) {
-			result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+				result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
 
-		} else {
-			_transition_command = int(_vehicle_cmd.param1 + 0.5f);
-		}
+			} else {
+				_transition_command = int(vehicle_command.param1 + 0.5f);
+			}
 
-		if (_vehicle_cmd.from_external) {
-			vehicle_command_ack_s command_ack{};
-			command_ack.timestamp = hrt_absolute_time();
-			command_ack.command = _vehicle_cmd.command;
-			command_ack.result = result;
-			command_ack.target_system = _vehicle_cmd.source_system;
-			command_ack.target_component = _vehicle_cmd.source_component;
+			if (vehicle_command.from_external) {
+				vehicle_command_ack_s command_ack{};
+				command_ack.timestamp = hrt_absolute_time();
+				command_ack.command = vehicle_command.command;
+				command_ack.result = result;
+				command_ack.target_system = vehicle_command.source_system;
+				command_ack.target_component = vehicle_command.source_component;
 
-			uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
-			command_ack_pub.publish(command_ack);
+				uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+				command_ack_pub.publish(command_ack);
+			}
 		}
 	}
 }

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -172,7 +172,6 @@ private:
 	position_setpoint_triplet_s		_pos_sp_triplet{};
 	tecs_status_s				_tecs_status{};
 	vehicle_attitude_s			_v_att{};				//vehicle attitude
-	vehicle_command_s			_vehicle_cmd{};
 	vehicle_control_mode_s			_v_control_mode{};	//vehicle control mode
 	vehicle_land_detected_s			_land_detected{};
 	vehicle_local_position_s		_local_pos{};
@@ -232,6 +231,4 @@ private:
 	void		vehicle_cmd_poll();
 
 	int 		parameters_update();			//Update local parameter cache
-
-	void		handle_command();
 };