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

GCS_MAVLink: re-worked text send in terms of ObjectArray

This commit is contained in:
Andrew Tridgell 2016-02-24 09:15:44 +11:00
parent 2120913ac2
commit fe3812c51b
2 changed files with 27 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -102,11 +102,10 @@ public:
}
struct statustext_t {
uint32_t timestamp_ms;
uint8_t bitmask;
mavlink_statustext_t msg;
};
static ObjectBuffer<statustext_t> _statustext_queue;
static ObjectArray<statustext_t> _statustext_queue;
// accessor for uart

72
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -27,7 +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);
ObjectArray<GCS_MAVLINK::statustext_t> GCS_MAVLINK::_statustext_queue(GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY);
GCS_MAVLINK::GCS_MAVLINK()
{
@ -1164,7 +1164,6 @@ void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, c
return;
}
statustext.timestamp_ms = AP_HAL::millis();
statustext.msg.severity = severity;
strncpy(statustext.msg.text, text, sizeof(statustext.msg.text));
@ -1172,6 +1171,9 @@ void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, c
// 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();
}
/*
send a statustext message to specific MAVLink connections in a bitmask
@ -1191,60 +1193,34 @@ void GCS_MAVLINK::service_statustext(void)
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;
for (uint8_t idx=0; idx<GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY; ) {
statustext_t *statustext = _statustext_queue[idx];
if (statustext == nullptr) {
break;
}
// 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);
uint8_t chan_bit = (1U<<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
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) {
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);
mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);
statustext->bitmask &= ~chan_bit;
}
}
}
if (statustext->bitmask == 0) {
_statustext_queue.remove(idx);
} else {
// move to next index
idx++;
}
}
} // while
}
/*