dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: STATUSTEXT stream use perf count for missed messages 

- don't send stale messages
 - process all queued messages until tx buffer is full
This commit is contained in:
Daniel Agar 2021-05-18 09:54:04 -04:00
parent 1f40a65210
commit 9969edfabc
1 changed files with 51 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/modules/mavlink/streams/STATUSTEXT.hpp
View File

@ -36,6 +36,8 @@
#include <uORB/topics/mavlink_log.h>
#include <lib/perf/perf_counter.h>
class MavlinkStreamStatustext : public MavlinkStream
{
public:
@ -55,57 +57,69 @@ public:
private:
explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {}
~MavlinkStreamStatustext()
{
perf_free(_missed_msg_count_perf);
}
uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)};
perf_counter_t _missed_msg_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": STATUSTEXT missed messages")};
uint16_t _id{0};
uint16_t _missed_msg_count{0};
bool send() override
{
if (_mavlink->is_connected() && (_mavlink->get_free_tx_buf() >= get_size())) {
if (_mavlink->is_connected()) {
while (_mavlink_log_sub.updated() && (_mavlink->get_free_tx_buf() >= get_size())) {
const unsigned last_generation = _mavlink_log_sub.get_last_generation();
const unsigned last_generation = _mavlink_log_sub.get_last_generation();
mavlink_log_s mavlink_log;
mavlink_log_s mavlink_log;
if (_mavlink_log_sub.update(&mavlink_log)) {
if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
_missed_msg_count++;
PX4_ERR("channel %d has missed %d mavlink log messages", _mavlink->get_channel(), _missed_msg_count);
}
if (_mavlink_log_sub.update(&mavlink_log)) {
// don't send stale messages
if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
mavlink_statustext_t msg{};
const char *text = mavlink_log.text;
constexpr unsigned max_chunk_size = sizeof(msg.text);
msg.severity = mavlink_log.severity;
msg.chunk_seq = 0;
msg.id = _id++;
unsigned text_size;
if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
perf_count(_missed_msg_count_perf);
PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
perf_event_count(_missed_msg_count_perf));
}
while ((text_size = strlen(text)) > 0) {
unsigned chunk_size = math::min(text_size, max_chunk_size);
mavlink_statustext_t msg{};
const char *text = mavlink_log.text;
constexpr unsigned max_chunk_size = sizeof(msg.text);
msg.severity = mavlink_log.severity;
msg.chunk_seq = 0;
msg.id = _id++;
unsigned text_size;
if (chunk_size < max_chunk_size) {
memcpy(&msg.text[0], &text[0], chunk_size);
// pad with zeros
memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
while ((text_size = strlen(text)) > 0) {
unsigned chunk_size = math::min(text_size, max_chunk_size);
} else {
memcpy(&msg.text[0], &text[0], chunk_size);
if (chunk_size < max_chunk_size) {
memcpy(&msg.text[0], &text[0], chunk_size);
// pad with zeros
memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
} else {
memcpy(&msg.text[0], &text[0], chunk_size);
}
mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
if (text_size <= max_chunk_size) {
break;
} else {
text += max_chunk_size;
}
msg.chunk_seq += 1;
}
return true;
}
mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
if (text_size <= max_chunk_size) {
break;
} else {
text += max_chunk_size;
}
msg.chunk_seq += 1;
}
return true;
}
}