From 333f539900f9d65ab6dd313f44e871fc2ecd4d1c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 13 Aug 2021 10:43:02 -0400 Subject: [PATCH] mavlink: add simple incoming message stats to status --- src/modules/mavlink/mavlink_main.cpp | 7 ++ src/modules/mavlink/mavlink_receiver.cpp | 125 ++++++++++++++++++++--- src/modules/mavlink/mavlink_receiver.h | 19 +++- 3 files changed, 133 insertions(+), 18 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c1a35de53b..6f0b80757f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2883,6 +2883,10 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { +#if !defined(CONSTRAINED_FLASH) + _receiver.enable_message_statistics(); +#endif // !CONSTRAINED_FLASH + if (_tstatus.heartbeat_type_gcs) { printf("\tGCS heartbeat valid\n"); } @@ -2922,7 +2926,10 @@ Mavlink::display_status() printf("\t tx rate max: %i B/s\n", _datarate); printf("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg); printf("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate); + +#if !defined(CONSTRAINED_FLASH) _receiver.print_detailed_rx_stats(); +#endif // !CONSTRAINED_FLASH if (_mavlink_ulog) { printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100., diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ec149d5ad1..f419f3489d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -76,6 +76,9 @@ MavlinkReceiver::~MavlinkReceiver() delete _px4_baro; delete _px4_gyro; delete _px4_mag; +#if !defined(CONSTRAINED_FLASH) + delete[] _received_msg_stats; +#endif // !CONSTRAINED_FLASH } MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : @@ -3170,6 +3173,10 @@ MavlinkReceiver::run() _mavlink->handle_message(&msg); update_rx_stats(msg); + + if (_message_statistics_enabled) { + update_message_statistics(msg); + } } } @@ -3268,7 +3275,6 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) _component_states[i].missed_messages += lost_messages; ++_component_states[i].received_messages; - _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; _component_states[i].last_sequence = message.seq; // Also update overall stats @@ -3282,7 +3288,6 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) _component_states[i].component_id = message.compid; ++_component_states[i].received_messages; - _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; _component_states[i].last_sequence = message.seq; _component_states_count = i + 1; @@ -3303,23 +3308,113 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) } } +void MavlinkReceiver::update_message_statistics(const mavlink_message_t &message) +{ +#if !defined(CONSTRAINED_FLASH) + + if (_received_msg_stats == nullptr) { + _received_msg_stats = new ReceivedMessageStats[MAX_MSG_STAT_SLOTS]; + } + + if (_received_msg_stats) { + const hrt_abstime now_ms = hrt_absolute_time() / 1000; + + int msg_stats_slot = -1; + bool reset_stats = false; + + // find matching msg id + for (int stat_slot = 0; stat_slot < MAX_MSG_STAT_SLOTS; stat_slot++) { + if ((_received_msg_stats[stat_slot].msg_id == message.msgid) + && (_received_msg_stats[stat_slot].system_id == message.sysid) + && (_received_msg_stats[stat_slot].component_id == message.compid)) { + + msg_stats_slot = stat_slot; + break; + } + } + + // otherwise find oldest or empty slot + if (msg_stats_slot < 0) { + uint32_t oldest_slot_time_ms = 0; + + for (int stat_slot = 0; stat_slot < MAX_MSG_STAT_SLOTS; stat_slot++) { + if (_received_msg_stats[stat_slot].last_time_received_ms <= oldest_slot_time_ms) { + oldest_slot_time_ms = _received_msg_stats[stat_slot].last_time_received_ms; + msg_stats_slot = stat_slot; + } + } + + reset_stats = true; + } + + if (msg_stats_slot >= 0) { + if (!reset_stats) { + if ((_received_msg_stats[msg_stats_slot].last_time_received_ms != 0) + && (now_ms > _received_msg_stats[msg_stats_slot].last_time_received_ms)) { + + float rate = 1000.f / (now_ms - _received_msg_stats[msg_stats_slot].last_time_received_ms); + + if (PX4_ISFINITE(_received_msg_stats[msg_stats_slot].avg_rate_hz)) { + _received_msg_stats[msg_stats_slot].avg_rate_hz = 0.9f * _received_msg_stats[msg_stats_slot].avg_rate_hz + 0.1f * rate; + + } else { + _received_msg_stats[msg_stats_slot].avg_rate_hz = rate; + } + + } else { + _received_msg_stats[msg_stats_slot].avg_rate_hz = 0.f; + } + + } else { + _received_msg_stats[msg_stats_slot].avg_rate_hz = NAN; + } + + _received_msg_stats[msg_stats_slot].last_time_received_ms = now_ms; + _received_msg_stats[msg_stats_slot].msg_id = message.msgid; + _received_msg_stats[msg_stats_slot].system_id = message.sysid; + _received_msg_stats[msg_stats_slot].component_id = message.compid; + } + } + +#endif // !CONSTRAINED_FLASH +} + void MavlinkReceiver::print_detailed_rx_stats() const { - const uint32_t now_ms = hrt_absolute_time() / 1000; - // TODO: add mutex around shared data. - for (unsigned i = 0; i < _component_states_count; ++i) { - if (_component_states[i].received_messages > 0) { - printf("\t received from sysid: %" PRIu8 " compid: %" PRIu8 ": %" PRIu32 ", lost: %" PRIu32 ", last %" PRIu32 - " ms ago\n", - _component_states[i].system_id, - _component_states[i].component_id, - _component_states[i].received_messages, - _component_states[i].missed_messages, - now_ms - _component_states[i].last_time_received_ms); + if (_component_states_count > 0) { + printf("\tReceived Messages:\n"); - } else { - break; + for (const auto &comp_stat : _component_states) { + if (comp_stat.received_messages > 0) { + printf("\t sysid:%3" PRIu8 ", compid:%3" PRIu8 ", Total: %" PRIu32 " (lost: %" PRIu32 ")\n", + comp_stat.system_id, comp_stat.component_id, + comp_stat.received_messages, comp_stat.missed_messages); + +#if !defined(CONSTRAINED_FLASH) + + if (_message_statistics_enabled && _received_msg_stats) { + for (int i = 0; i < MAX_MSG_STAT_SLOTS; i++) { + const ReceivedMessageStats &msg_stat = _received_msg_stats[i]; + + const uint32_t now_ms = hrt_absolute_time() / 1000; + + // valid messages received within the last 10 seconds + if ((msg_stat.system_id == comp_stat.system_id) + && (msg_stat.component_id == comp_stat.component_id) + && (msg_stat.last_time_received_ms != 0) + && (now_ms - msg_stat.last_time_received_ms < 10'000)) { + + const float elapsed_s = (now_ms - msg_stat.last_time_received_ms) / 1000.f; + + printf("\t msgid:%5" PRIu16 ", Rate:%5.1f Hz, last %.2fs ago\n", + msg_stat.msg_id, (double)msg_stat.avg_rate_hz, (double)elapsed_s); + } + } + } + +#endif // !CONSTRAINED_FLASH + } } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 70957dd934..3d9d051030 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -127,6 +127,7 @@ public: void stop(); bool component_was_seen(int system_id, int component_id); + void enable_message_statistics() { _message_statistics_enabled = true; } void print_detailed_rx_stats() const; private: @@ -143,8 +144,7 @@ private: const vehicle_command_s &vehicle_command); uint8_t handle_request_message_command(uint16_t message_id, float param2 = 0.0f, float param3 = 0.0f, - float param4 = 0.0f, - float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); + float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); void handle_message(mavlink_message_t *msg); @@ -228,6 +228,7 @@ private: void schedule_tune(const char *tune); + void update_message_statistics(const mavlink_message_t &message); void update_rx_stats(const mavlink_message_t &message); px4::atomic_bool _should_exit{false}; @@ -251,7 +252,6 @@ private: static constexpr unsigned MAX_REMOTE_COMPONENTS{8}; struct ComponentState { - uint32_t last_time_received_ms{0}; uint32_t received_messages{0}; uint32_t missed_messages{0}; uint8_t system_id{0}; @@ -262,6 +262,19 @@ private: unsigned _component_states_count{0}; bool _warned_component_states_full_once{false}; + bool _message_statistics_enabled {false}; +#if !defined(CONSTRAINED_FLASH) + static constexpr int MAX_MSG_STAT_SLOTS {16}; + struct ReceivedMessageStats { + float avg_rate_hz{0.f}; // average rate + uint32_t last_time_received_ms{0}; + uint16_t msg_id{0}; + uint8_t system_id{0}; + uint8_t component_id{0}; + }; + ReceivedMessageStats *_received_msg_stats{nullptr}; +#endif // !CONSTRAINED_FLASH + uint64_t _total_received_counter{0}; ///< The total number of successfully received messages uint64_t _total_lost_counter{0}; ///< Total messages lost during transmission.