Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: create queue scheme for static statustext msgs

Browse Source
This commit is contained in:
Tom Pittenger 2016-02-23 12:50:57 -08:00 committed by Andrew Tridgell
parent 09dd2a1b3d
commit 54d2a263fe
2 changed files with 141 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/GCS_MAVLink/GCS.h
View File

@ -17,12 +17,19 @@
#include "MAVLink_routing.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Mount/AP_Mount.h>
#include <AP_HAL/utility/RingBuffer.h>
// 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_t> _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);

149
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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_t> 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_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= 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<<dest_chan), text);
}
/*
send a statustext text string to specific MAVLink bitmask
*/
void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text)
{
if (dataflash_p != NULL) {
dataflash_p->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_COMM_NUM_BUFFERS; i++) {
uint8_t chan_bit = (1<<i);
// logical AND (&) to mask them together
if (statustext.bitmask & chan_bit) { // something is queued on a port and that's the port index we're looped at
mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan_index) > 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
}
/*