diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index d3f0cb1d8b..6f4cbd577a 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -66,6 +66,11 @@ MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_sys _next_rate_check = _last_sent_time + _rate_calculation_delta_t * 1.e6f; } +MavlinkULog::~MavlinkULog() +{ + perf_free(_msg_missed_ulog_stream_perf); +} + void MavlinkULog::start_ack_received() { if (_waiting_for_initial_ack) { @@ -134,7 +139,7 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) _ulog_stream_sub.update(); if (_ulog_stream_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("ulog_stream lost, generation %d -> %d", last_generation, _ulog_stream_sub.get_last_generation()); + perf_count(_msg_missed_ulog_stream_perf); } const ulog_stream_s &ulog_data = _ulog_stream_sub.get(); diff --git a/src/modules/mavlink/mavlink_ulog.h b/src/modules/mavlink/mavlink_ulog.h index c0b81b1998..9c26329792 100644 --- a/src/modules/mavlink/mavlink_ulog.h +++ b/src/modules/mavlink/mavlink_ulog.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -102,7 +103,7 @@ private: MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component); - ~MavlinkULog() = default; + ~MavlinkULog(); static void lock() { @@ -137,6 +138,8 @@ private: int _current_num_msgs = 0; ///< number of messages sent within the current time interval hrt_abstime _next_rate_check; ///< next timestamp at which to update the rate + perf_counter_t _msg_missed_ulog_stream_perf{perf_alloc(PC_COUNT, MODULE_NAME": ulog_stream messages missed")}; + /* do not allow copying this class */ MavlinkULog(const MavlinkULog &) = delete; MavlinkULog operator=(const MavlinkULog &) = delete;