From 78741c87e5415c9e894f619e28b145e127576a56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Mar 2015 23:16:45 -0700 Subject: MAVLink app: 1) only transmit active params, 2) send params faster, 3) ensure no overflow occurs on buffer when sending at higher rate. --- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 30 +++++++++++++++--------------- src/modules/mavlink/mavlink_parameters.cpp | 23 +++++++++++++++++++---- 3 files changed, 35 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f8e819ce7..632c8c709 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1361,7 +1361,7 @@ Mavlink::task_main(int argc, char *argv[]) /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); - _parameters_manager->set_interval(interval_from_rate(30.0f)); + _parameters_manager->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _parameters_manager); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index baaa7bc13..6eef594f5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -143,6 +143,13 @@ public: bool get_forwarding_on() { return _forwarding_on; } + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static int start_helper(int argc, char *argv[]); /** @@ -162,12 +169,12 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const uint8_t msgid, const void *msg); + void send_message(const uint8_t msgid, const void *msg); /** * Resend message as is, don't change sequence number and CRC. */ - void resend_message(mavlink_message_t *msg); + void resend_message(mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -309,7 +316,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -331,9 +338,9 @@ private: unsigned _bytes_txerr; unsigned _bytes_rx; uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; + float _rate_tx; + float _rate_txerr; + float _rate_rx; struct telemetry_status_s _rstatus; ///< receive status @@ -363,16 +370,9 @@ private: void mavlink_update_system(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - static unsigned int interval_from_rate(float rate); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e910f674a..e7e0c11df 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -184,8 +184,23 @@ MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested */ if (_send_all_index >= 0) { - send_param(param_for_index(_send_all_index)); - _send_all_index++; + + /* skip if no space is available */ + if (_mavlink->get_free_tx_buf() < get_size()) { + return; + } + + /* look for the first parameter which is used */ + param_t p; + do { + p = param_for_index(_send_all_index); + _send_all_index++; + } while (p != PARAM_INVALID && !param_used(p)); + + if (p != PARAM_INVALID) { + send_param(p); + } + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } @@ -209,8 +224,8 @@ MavlinkParametersManager::send_param(param_t param) return; } - msg.param_count = param_count(); - msg.param_index = param_get_index(param); + msg.param_count = param_count_used(); + msg.param_index = param_get_used_index(param); /* copy parameter name */ strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); -- cgit v1.2.3