Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: split statustexts into 50-byte chunks using mav2 extensions

This commit is contained in:
Peter Barker 2019-12-08 21:37:10 +11:00 committed by Peter Barker
parent a1f0452940
commit e248bdfb16
2 changed files with 74 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -930,6 +930,7 @@ private:
uint8_t bitmask;
mavlink_statustext_t msg;
};
char statustext_printf_buffer[256+1];
virtual AP_GPS::GPS_Status min_status_for_gps_healthy() const {
// NO_FIX simply excludes NO_GPS

112
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1763,56 +1763,90 @@ void GCS_MAVLINK::send_ahrs()
*/
void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t dest_bitmask)
{
char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
if (hal.util->vsnprintf(text, sizeof(text), fmt, arg_list) > MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Statustext (%s) too long", text);
#endif
}
char first_piece_of_text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]{};
do {
WITH_SEMAPHORE(_statustext_sem);
// send_text can be called from multiple threads; we must
// protect the "text" member with _statustext_sem
hal.util->vsnprintf(statustext_printf_buffer, sizeof(statustext_printf_buffer), fmt, arg_list);
memcpy(first_piece_of_text, statustext_printf_buffer, ARRAY_SIZE(first_piece_of_text));
// filter destination ports to only allow active ports.
statustext_t statustext{};
if (update_send_has_been_called) {
statustext.bitmask = (GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask() );
} else {
// we have not yet initialised the streaming-channel-mask,
// which is done as part of the update() call. So just send
// it to all channels:
statustext.bitmask = (1<<_num_gcs)-1;
}
statustext.bitmask &= dest_bitmask;
if (!statustext.bitmask) {
// nowhere to send
break;
}
statustext.msg.severity = severity;
static uint16_t msgid;
if (strlen(statustext_printf_buffer) > sizeof(statustext.msg.text)) {
msgid++;
if (msgid == 0) {
msgid = 1;
}
statustext.msg.id = msgid;
}
const char *remainder = statustext_printf_buffer;
for (uint8_t i=0; i<_status_capacity; i++) {
statustext.msg.chunk_seq = i;
const size_t remainder_len = strlen(remainder);
// note that remainder_len may be zero here!
uint16_t n = MIN(sizeof(statustext.msg.text), remainder_len);
if (i == _status_capacity -1 && n == sizeof(statustext.msg.text)) {
// fantastic. This us a very long statustext and
// we've actually managed to push everything else out
// of the queue - this is the last chunk, so we MUST
// null-terminate.
n -= 1;
}
memset(statustext.msg.text, '\0', sizeof(statustext.msg.text));
memcpy(statustext.msg.text, remainder, n);
// 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);
remainder = &remainder[n];
// note that remainder_len here is the remainder length for
// the *old* remainder!
if (remainder_len < sizeof(statustext.msg.text) || statustext.msg.id == 0) {
break;
}
}
// try and send immediately if possible
service_statustext();
} while (false);
// given we don't really know what these methods get up to, we
// don't hold the statustext semaphore while doing them:
AP_Logger *logger = AP_Logger::get_singleton();
if (logger != nullptr) {
logger->Write_Message(text);
logger->Write_Message(first_piece_of_text);
}
frsky = AP::frsky_telem();
if (frsky != nullptr) {
frsky->queue_message(severity, text);
frsky->queue_message(severity, first_piece_of_text);
}
AP_Notify *notify = AP_Notify::get_singleton();
if (notify) {
notify->send_text(text);
notify->send_text(first_piece_of_text);
}
// filter destination ports to only allow active ports.
statustext_t statustext{};
if (update_send_has_been_called) {
statustext.bitmask = (GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask() );
} else {
// we have not yet initialised the streaming-channel-mask,
// which is done as part of the update() call. So just send
// it to all channels:
statustext.bitmask = (1<<_num_gcs)-1;
}
statustext.bitmask &= dest_bitmask;
if (!statustext.bitmask) {
// nowhere to send
return;
}
statustext.msg.severity = severity;
strncpy(statustext.msg.text, text, sizeof(statustext.msg.text));
WITH_SEMAPHORE(_statustext_sem);
// 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);
// try and send immediately if possible
service_statustext();
}
/*
@ -1848,7 +1882,7 @@ void GCS::service_statustext(void)
mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (HAVE_PAYLOAD_SPACE(chan_index, STATUSTEXT)) {
// 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);
mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text, statustext->msg.id, statustext->msg.chunk_seq);
statustext->bitmask &= ~chan_bit;
}
}