From 2f113a4ce3138d3ec9b595636aa0656b47a31ebf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Jun 2016 16:38:53 +0200 Subject: [PATCH] MAVLink: Allow data rate updates --- src/modules/mavlink/mavlink_main.h | 3 +++ src/modules/mavlink/mavlink_receiver.cpp | 8 ++++++-- src/modules/mavlink/mavlink_receiver.h | 6 +++++- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6ae5636317..b3b15d3a2f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -417,6 +417,9 @@ public: void set_logging_enabled(bool logging) { _logging_enabled = logging; } + int get_data_rate() { return _datarate; } + void set_data_rate(int rate) { if (rate > 0) _datarate = rate; } + protected: Mavlink *next; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1841861298..f5c5516d5a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -360,7 +360,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { - set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2); + set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3); } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { get_message_interval((int)cmd_mavlink.param1); @@ -1402,8 +1402,12 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) } void -MavlinkReceiver::set_message_interval(int msgId, float interval) +MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) { + if (data_rate > 0) { + _mavlink->set_data_rate(data_rate); + } + // configure_stream wants a rate (msgs/second), so convert here. float rate = 0; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 45e904d295..bf0a5121ed 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -148,8 +148,12 @@ private: /** * Set the interval at which the given message stream is published. * The rate is the number of messages per second. + * + * @param msgId the message ID of to change the interval of + * @param interval the interval in us to send the message at + * @param data_rate the total link data rate in bytes per second */ - void set_message_interval(int msgId, float rate); + void set_message_interval(int msgId, float interval, int data_rate=-1); void get_message_interval(int msgId); /**