From 631f314e89ea61f62fbca77335fa53511bdea661 Mon Sep 17 00:00:00 2001 From: acfloria Date: Thu, 14 Dec 2017 14:00:09 +0100 Subject: [PATCH] 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 | 1 + src/modules/mavlink/mavlink_main.cpp | 28 +++++++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 3 ++- 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 1ea206423f..baa6ef35d1 100644 --- a/msg/vehicle_command.msg +++ b/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 | diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9aa235c247..9f947cade9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #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: "); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6e1ac495c6..e14ec482c0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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;