fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: add simple incoming message stats to status

Browse Source
This commit is contained in:
Daniel Agar 2021-08-13 10:43:02 -04:00
parent feff564882
commit 333f539900
3 changed files with 133 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/mavlink/mavlink_main.cpp
View File

@ -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.,

125
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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
}
}
}
}

19
src/modules/mavlink/mavlink_receiver.h
View File

@ -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.