diff --git a/src/modules/mavlink/streams/STATUSTEXT.hpp b/src/modules/mavlink/streams/STATUSTEXT.hpp index 312f0ccffb..0ee6e45543 100644 --- a/src/modules/mavlink/streams/STATUSTEXT.hpp +++ b/src/modules/mavlink/streams/STATUSTEXT.hpp @@ -36,6 +36,8 @@ #include +#include + 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); - mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); + } else { + memcpy(&msg.text[0], &text[0], chunk_size); + } - if (text_size <= max_chunk_size) { - break; + mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); - } else { - text += max_chunk_size; - } + if (text_size <= max_chunk_size) { + break; - msg.chunk_seq += 1; - } + } else { + text += max_chunk_size; + } + + msg.chunk_seq += 1; + } - return true; + return true; + } + } } }