From 54d2a263fee489b5710ba9d04a76cfaed57de499 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 23 Feb 2016 12:50:57 -0800 Subject: [PATCH] GCS_MAVLink: create queue scheme for static statustext msgs --- libraries/GCS_MAVLink/GCS.h | 27 +++-- libraries/GCS_MAVLink/GCS_Common.cpp | 149 ++++++++++++++++++++++----- 2 files changed, 141 insertions(+), 35 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 26664a2306..ac403a73ed 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -17,12 +17,19 @@ #include "MAVLink_routing.h" #include #include +#include // check if a message will fit in the payload space available #define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) #define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false +#if HAL_CPU_CLASS <= HAL_CPU_CLASS_150 || CONFIG_HAL_BOARD == HAL_BOARD_SITL + #define GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY 5 +#else + #define GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY 30 +#endif + // GCS Message ID's /// NOTE: to ensure we never block on sending MAVLink messages /// please keep each MSG_ to a single MAVLink message. If need be @@ -94,6 +101,14 @@ public: msg_snoop = _msg_snoop; } + struct statustext_t { + uint32_t timestamp_ms; + uint8_t bitmask; + mavlink_statustext_t msg; + }; + static ObjectBuffer _statustext_queue; + + // accessor for uart AP_HAL::UARTDriver *get_uart() { return _port; } @@ -119,10 +134,6 @@ public: // see if we should send a stream now. Called at 50Hz bool stream_trigger(enum streams stream_num); - // this costs us 51 bytes per instance, but means that low priority - // messages don't block the CPU - mavlink_statustext_t pending_status; - // call to reset the timeout window for entering the cli void reset_cli_timeout(); @@ -160,11 +171,13 @@ public: static uint8_t active_channel_mask(void) { return mavlink_active; } /* - send a statustext message to all active MAVLink - connections. This function is static so it can be called from - any library + send a statustext message to active MAVLink connections, or a specific + one. This function is static so it can be called from any library. */ static void send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...); + static void send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan, const char *fmt, ...); + static void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text); + static void service_statustext(void); // send a PARAM_VALUE message to all active MAVLink connections. static void send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 358979a748..5dee497be0 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal; uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms; uint8_t GCS_MAVLINK::mavlink_active = 0; +ObjectBuffer GCS_MAVLINK::_statustext_queue(GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY); GCS_MAVLINK::GCS_MAVLINK() { @@ -584,20 +585,7 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str) { - if (severity < MAV_SEVERITY_WARNING && - comm_get_txspace(chan) >= - MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) { - // send immediately - char msg[50] {}; - strncpy(msg, str, sizeof(msg)); - mavlink_msg_statustext_send(chan, severity, msg); - } else { - // send via the deferred queuing system - mavlink_statustext_t *s = &pending_status; - s->severity = (uint8_t)severity; - strncpy((char *)s->text, str, sizeof(s->text)); - send_message(MSG_STATUSTEXT); - } + GCS_MAVLINK::send_statustext_chan(severity, chan, str); } void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio) @@ -1137,21 +1125,126 @@ void GCS_MAVLINK::send_ahrs(AP_AHRS &ahrs) */ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...) { - for (uint8_t i=0; i= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) { - char msg2[50] {}; - va_list arg_list; - va_start(arg_list, fmt); - hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list); - va_end(arg_list); - mavlink_msg_statustext_send(chan, - severity, - msg2); - } - } + char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {}; + va_list arg_list; + va_start(arg_list, fmt); + hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list); + va_end(arg_list); + send_statustext(severity, mavlink_active, text); +} + +/* + send a statustext message to specific MAVLink channel, zero indexed +*/ +void GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan, const char *fmt, ...) +{ + char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {}; + va_list arg_list; + va_start(arg_list, fmt); + hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list); + va_end(arg_list); + send_statustext(severity, (1<Log_Write_Message(text); } + + // filter destination ports to only allow active ports. + statustext_t statustext{}; + statustext.bitmask = mavlink_active & dest_bitmask; + if (!statustext.bitmask) { + // nowhere to send + return; + } + + statustext.timestamp_ms = AP_HAL::millis(); + statustext.msg.severity = severity; + strncpy(statustext.msg.text, text, sizeof(statustext.msg.text)); + + // The force push will ensure comm links do not block other comm links forever if they fail. + // If we push to a full buffer then we overwrite the oldest entry, effectively removing the + // block but not until the buffer fills up. + _statustext_queue.push_force(statustext); +} +/* + send a statustext message to specific MAVLink connections in a bitmask + */ +void GCS_MAVLINK::service_statustext(void) +{ + // create bitmask of what mavlink ports we should send this text to. + // note, if sending to all ports, we only need to store the bitmask for each and the string only once. + // once we send over a link, clear the port but other busy ports bit may stay allowing for faster links + // to clear the bit and send quickly but slower links to still store the string. Regardless of mixed + // bitrates of ports, a maximum of GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY strings can be buffered. Downside + // is if you have a super slow link mixed with a faster port, if there are GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY + // strings in the slow queue then the next item can not be queued for the faster link + + if (_statustext_queue.empty()) { + // nothing to do + return; + } + + // keep sending until all channels have unable to send (full) or have nothing to send + bool still_sending = true; + + // using 'while' to ensure we either run out of text to send or all buffers on all ports are full + while (still_sending) { + still_sending = false; // if we send something, keep going + + // populate statustext + statustext_t statustext; // no need to {} init it because the pop will memcpy all bytes and when it was loaded it was properly zero initialized + if (!_statustext_queue.peek(statustext)) { + // all done, nothing else to send + return; + } + + // try and send to all active mavlink ports listed in the statustext.bitmask + for (uint8_t i=0; i MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) { + // we have space so send then clear that channel bit on the mask + mavlink_msg_statustext_send(chan_index, statustext.msg.severity, statustext.msg.text); + statustext.bitmask &= ~chan_bit; + still_sending = true; + + } else if (AP_HAL::millis() - statustext.timestamp_ms > 2000) { + // port has been blocking for 2 seconds. This is blocking all further messages on all + // ports so its best to toss the packet by clearing bitmask. + statustext.bitmask &= ~chan_bit; + + // Also, forget that mavlink port. The port will be re-remembered if we get any data from it + mavlink_active &= ~chan_bit; + + // restart loop so the other blocked ports can send immediately. + still_sending = true; + + send_statustext_all(MAV_SEVERITY_CRITICAL, "Can not send statustext on Mavlink port %i", i); + } // if comm + } // if bitmask + } // for i + + if (!statustext.bitmask) { + // throw away what we just peeked at, all done with it + _statustext_queue.pop(); + + } else if (still_sending) { + // something was sent so the bitmask changed but there is still something pending. + // Perform an in-place update to the peeked entry index. Only the bitmask should be changing + _statustext_queue.update(statustext); + } + } // while } /*