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)

platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp

@@ -39,7 +39,7 @@
 #include <containers/IntrusiveQueue.hpp>
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
-
+#include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 
 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:
 

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

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<ScheduledWorkItem *>(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 {

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

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();

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);