From 8ee0a5d328ebe59ce65d37bbe48ab4e10fbe1665 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 19 May 2020 12:49:42 -0400 Subject: [PATCH] px4_work_queue: minor status changes - only record start time on first run rather than init - increase name length - round average interval to nearest microsecond - basic formatting consistency (google style guide) --- .../px4_work_queue/WorkItem.hpp | 16 +++++--- .../px4_work_queue/WorkQueue.hpp | 2 +- .../px4_work_queue/ScheduledWorkItem.cpp | 20 +++------- platforms/common/px4_work_queue/WorkItem.cpp | 40 ++++++++----------- platforms/common/px4_work_queue/WorkQueue.cpp | 34 ++++++---------- .../px4_work_queue/WorkQueueManager.cpp | 4 +- 6 files changed, 49 insertions(+), 67 deletions(-) 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 14de3fbbe1..9b9d5a26b7 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 @@ -39,7 +39,7 @@ #include #include #include - +#include #include namespace px4 @@ -91,7 +91,14 @@ protected: void ScheduleClear(); protected: - void RunPreamble() { _run_count++; } + void RunPreamble() + { + _run_count++; + + if (_time_first_run == 0) { + _time_first_run = hrt_absolute_time(); + } + } friend void WorkQueue::Run(); virtual void Run() = 0; @@ -111,10 +118,9 @@ protected: float average_rate() const; float average_interval() const; - - hrt_abstime _start_time{0}; - unsigned _run_count{0}; + hrt_abstime _time_first_run{0}; const char *_item_name; + uint32_t _run_count{0}; private: diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index 4815c5cf2f..7366b91048 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -76,7 +76,7 @@ private: bool should_exit() const { return _should_exit.load(); } - inline void signal_worker_thread(); + inline void SignalWorkerThread(); #ifdef __PX4_NUTTX // In NuttX work can be enqueued from an ISR diff --git a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp index 43152ce5eb..7b01a77aee 100644 --- a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -41,41 +41,33 @@ ScheduledWorkItem::~ScheduledWorkItem() ScheduleClear(); } -void -ScheduledWorkItem::schedule_trampoline(void *arg) +void ScheduledWorkItem::schedule_trampoline(void *arg) { ScheduledWorkItem *dev = static_cast(arg); dev->ScheduleNow(); } -void -ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us) +void ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us) { hrt_call_after(&_call, delay_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); } -void -ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us) +void ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us) { - // reset start time to first deadline (approximately) - _start_time = hrt_absolute_time() + interval_us + delay_us; - hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); } -void -ScheduledWorkItem::ScheduleClear() +void ScheduledWorkItem::ScheduleClear() { // first clear any scheduled hrt call, then remove the item from the runnable queue hrt_cancel(&_call); WorkItem::ScheduleClear(); } -void -ScheduledWorkItem::print_run_status() const +void ScheduledWorkItem::print_run_status() const { if (_call.period > 0) { - PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), + PX4_INFO_RAW("%-26s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), (double)average_interval(), _call.period); } else { diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index af4f3a57fd..63581f2928 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -57,7 +57,6 @@ WorkItem::WorkItem(const char *name, const WorkItem &work_item) : if ((wq != nullptr) && wq->Attach(this)) { _wq = wq; - _start_time = hrt_absolute_time(); } } @@ -66,8 +65,7 @@ WorkItem::~WorkItem() Deinit(); } -bool -WorkItem::Init(const wq_config_t &config) +bool WorkItem::Init(const wq_config_t &config) { // clear any existing first Deinit(); @@ -76,7 +74,7 @@ WorkItem::Init(const wq_config_t &config) if ((wq != nullptr) && wq->Attach(this)) { _wq = wq; - _start_time = hrt_absolute_time(); + _time_first_run = 0; return true; } @@ -84,8 +82,7 @@ WorkItem::Init(const wq_config_t &config) return false; } -void -WorkItem::Deinit() +void WorkItem::Deinit() { // remove any currently queued work if (_wq != nullptr) { @@ -100,49 +97,44 @@ WorkItem::Deinit() } } -void -WorkItem::ScheduleClear() +void WorkItem::ScheduleClear() { if (_wq != nullptr) { _wq->Remove(this); } } -float -WorkItem::elapsed_time() const +float WorkItem::elapsed_time() const { - return hrt_elapsed_time(&_start_time) / 1e6f; + return hrt_elapsed_time(&_time_first_run) / 1e6f; } -float -WorkItem::average_rate() const +float WorkItem::average_rate() const { const float rate = _run_count / elapsed_time(); - if ((_run_count > 0) && PX4_ISFINITE(rate)) { + if ((_run_count > 1) && PX4_ISFINITE(rate)) { return rate; } - return 0.0f; + return 0.f; } -float -WorkItem::average_interval() const +float WorkItem::average_interval() const { const float rate = average_rate(); - const float interval = 1000000.0f / rate; + const float interval = 1e6f / rate; - if ((rate > 0.0f) && PX4_ISFINITE(interval)) { - return interval; + if ((rate > FLT_EPSILON) && PX4_ISFINITE(interval)) { + return roundf(interval); } - return 0.0f; + return 0.f; } -void -WorkItem::print_run_status() const +void WorkItem::print_run_status() const { - PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us\n", _item_name, (double)average_rate(), (double)average_interval()); + PX4_INFO_RAW("%-26s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval()); } } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index f6bd1a769c..8fe345ff9a 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -72,8 +72,7 @@ WorkQueue::~WorkQueue() #endif /* __PX4_NUTTX */ } -bool -WorkQueue::Attach(WorkItem *item) +bool WorkQueue::Attach(WorkItem *item) { work_lock(); @@ -87,8 +86,7 @@ WorkQueue::Attach(WorkItem *item) return false; } -void -WorkQueue::Detach(WorkItem *item) +void WorkQueue::Detach(WorkItem *item) { work_lock(); @@ -99,24 +97,22 @@ WorkQueue::Detach(WorkItem *item) PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name); request_stop(); - signal_worker_thread(); + SignalWorkerThread(); } work_unlock(); } -void -WorkQueue::Add(WorkItem *item) +void WorkQueue::Add(WorkItem *item) { work_lock(); _q.push(item); work_unlock(); - signal_worker_thread(); + SignalWorkerThread(); } -void -WorkQueue::signal_worker_thread() +void WorkQueue::SignalWorkerThread() { int sem_val; @@ -125,16 +121,14 @@ WorkQueue::signal_worker_thread() } } -void -WorkQueue::Remove(WorkItem *item) +void WorkQueue::Remove(WorkItem *item) { work_lock(); _q.remove(item); work_unlock(); } -void -WorkQueue::Clear() +void WorkQueue::Clear() { work_lock(); @@ -145,8 +139,7 @@ WorkQueue::Clear() work_unlock(); } -void -WorkQueue::Run() +void WorkQueue::Run() { while (!should_exit()) { // loop as the wait may be interrupted by a signal @@ -171,12 +164,11 @@ WorkQueue::Run() PX4_DEBUG("%s: exiting", _config.name); } -void -WorkQueue::print_status(bool last) +void WorkQueue::print_status(bool last) { const size_t num_items = _work_items.size(); PX4_INFO_RAW("%-16s\n", get_name()); - size_t i = 0; + unsigned i = 0; for (WorkItem *item : _work_items) { i++; @@ -189,10 +181,10 @@ WorkQueue::print_status(bool last) } if (i < num_items) { - PX4_INFO_RAW("|__ %zu) ", i); + PX4_INFO_RAW("|__%2d) ", i); } else { - PX4_INFO_RAW("\\__ %zu) ", i); + PX4_INFO_RAW("\\__%2d) ", i); } item->print_run_status(); diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 4c537fd49c..efbabe0a9a 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -390,7 +390,7 @@ WorkQueueManagerStatus() if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) { const size_t num_wqs = _wq_manager_wqs_list->size(); - PX4_INFO_RAW("\nWork Queue: %-1zu threads RATE INTERVAL\n", num_wqs); + PX4_INFO_RAW("\nWork Queue: %-1zu threads RATE INTERVAL\n", num_wqs); LockGuard lg{_wq_manager_wqs_list->mutex()}; size_t i = 0; @@ -398,7 +398,7 @@ WorkQueueManagerStatus() for (WorkQueue *wq : *_wq_manager_wqs_list) { i++; - const bool last_wq = !(i < num_wqs); + const bool last_wq = (i >= num_wqs); if (!last_wq) { PX4_INFO_RAW("|__ %zu) ", i);