diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp index 6789d4bea4..4975dc1220 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp @@ -69,7 +69,7 @@ protected: ScheduledWorkItem(const char *name, const wq_config_t &config) : WorkItem(name, config) {} virtual ~ScheduledWorkItem() override; - virtual void print_run_status() const override; + virtual void print_run_status() override; private: diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 58a073a9b0..2f95b064c4 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -70,7 +70,7 @@ public: } } - virtual void print_run_status() const; + virtual void print_run_status(); /** * Switch to a different WorkQueue. @@ -99,10 +99,12 @@ protected: void RunPreamble() { - _run_count++; - - if (_time_first_run == 0) { + if (_run_count == 0) { _time_first_run = hrt_absolute_time(); + _run_count = 1; + + } else { + _run_count++; } } diff --git a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp index 7b01a77aee..e7ca6379b2 100644 --- a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -64,7 +64,7 @@ void ScheduledWorkItem::ScheduleClear() WorkItem::ScheduleClear(); } -void ScheduledWorkItem::print_run_status() const +void ScheduledWorkItem::print_run_status() { if (_call.period > 0) { PX4_INFO_RAW("%-26s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index 63581f2928..0c4be6a15b 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -132,9 +132,12 @@ float WorkItem::average_interval() const return 0.f; } -void WorkItem::print_run_status() const +void WorkItem::print_run_status() { PX4_INFO_RAW("%-26s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval()); + + // reset statistics + _run_count = 0; } } // namespace px4