En-/disable transmitting from a mavlink instance using a uORB message

Allow enabling/disabling transmitting data from all mavlink instances
with a specific mode/type. When disabled the instance can still receive
data. The uORB message used is the vehicle_command and uses the following
parameter:
param1: Specifies the mode/type of the instances to enable/disable transmitting
param2: Boolean to indicate if transmitting should be enabled or disabled

msg/vehicle_command.msg

@@ -72,6 +72,7 @@ uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed pay
 uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
 uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
+uint16 VEHICLE_CMD_MAVLINK_ENABLE_SENDING = 2600	# Start/Stop transmitting data from all instances in mavlink from a certain type
 
 uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |

src/modules/mavlink/mavlink_main.cpp

@@ -75,6 +75,7 @@
 
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/mavlink_log.h>
 
 #include "mavlink_bridge_header.h"
@@ -192,6 +193,7 @@ Mavlink::Mavlink() :
 	_task_should_exit(false),
 	next(nullptr),
 	_instance_id(0),
+	_transmitting_enabled(true),
 	_mavlink_log_pub(nullptr),
 	_task_running(false),
 	_mavlink_buffer{},
@@ -1957,6 +1959,8 @@ Mavlink::task_main(int argc, char *argv[])
 	/* Initialize system properties */
 	mavlink_update_system();
 
+	MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command));
+	uint64_t cmd_time = 0;
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
 	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
@@ -1966,14 +1970,19 @@ Mavlink::task_main(int argc, char *argv[])
 	/* We don't want to miss the first advertise of an ACK, so we subscribe from the
 	 * beginning and not just when the topic exists. */
 	ack_sub->subscribe_from_beginning(true);
+	cmd_sub->subscribe_from_beginning(true);
 
 	MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
 
 	struct vehicle_status_s status;
 	status_sub->update(&status_time, &status);
 
-	/* add default streams depending on mode */
+	/* Activate sending the data by default except for the IRIDIUM mode */
+	_transmitting_enabled = true;
+	if (_mode == MAVLINK_MODE_IRIDIUM)
+		_transmitting_enabled = false;
 
+	/* add default streams depending on mode */
 	if (_mode != MAVLINK_MODE_IRIDIUM) {
 
 		/* HEARTBEAT is constant rate stream, rate never adjusted */
@@ -2186,6 +2195,21 @@ Mavlink::task_main(int argc, char *argv[])
 			set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
 		}
 
+		struct vehicle_command_s vehicle_cmd;
+		if (cmd_sub->update(&cmd_time, &vehicle_cmd)) {
+			if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING) {
+				if (_mode == (int)round(vehicle_cmd.param1)) {
+					if (_transmitting_enabled != (int)vehicle_cmd.param2) {
+						if ((int)vehicle_cmd.param2)
+							PX4_INFO("Enable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
+						else
+							PX4_INFO("Disable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
+						_transmitting_enabled = (int)vehicle_cmd.param2;
+					}
+				}
+			}
+		}
+
 		/* send command ACK */
 		uint16_t current_command_ack = 0;
 		struct vehicle_command_ack_s command_ack;
@@ -2593,6 +2617,8 @@ Mavlink::display_status()
 	}
 
 	printf("\taccepting commands: %s, FTP enabled: %s\n", accepting_commands() ? "YES" : "NO", _ftp_on ? "YES" : "NO");
+	printf("\ttransmitting enabled: %s\n", _transmitting_enabled ? "YES" : "NO");
+	printf("\tmode: %s\n", mavlink_mode_str(_mode));
 	printf("\tMAVLink version: %i\n", _protocol_version);
 
 	printf("\ttransport protocol: ");

src/modules/mavlink/mavlink_main.h

@@ -386,7 +386,7 @@ public:
 	bool			get_has_received_messages() { return _received_messages; }
 	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
 	bool			get_wait_to_transmit() { return _wait_to_transmit; }
-	bool			should_transmit() { return (_boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
+	bool			should_transmit() { return (_transmitting_enabled && _boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
 
 	bool			message_buffer_write(const void *ptr, int size);
 
@@ -485,6 +485,7 @@ protected:
 
 private:
 	int			_instance_id;
+	bool		_transmitting_enabled;
 
 	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;