From c8f77d438d11bc25298983edac3b1a8e68943e3d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 2 Sep 2019 20:41:34 -0400
Subject: [PATCH] load_mon: update orb_publish to uORB::Publication<>

---
 msg/task_stack_info.msg           |  2 ++
 src/modules/load_mon/load_mon.cpp | 23 +++++++----------------
 2 files changed, 9 insertions(+), 16 deletions(-)

diff --git a/msg/task_stack_info.msg b/msg/task_stack_info.msg
index c142a9a752..bb69bf1a88 100644
--- a/msg/task_stack_info.msg
+++ b/msg/task_stack_info.msg
@@ -4,3 +4,5 @@ uint64 timestamp		# time since system start (microseconds)
 
 uint16 stack_free
 char[24] task_name
+
+uint8 ORB_QUEUE_LENGTH = 2
diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp
index a2b82ae15a..7f4e10b0de 100644
--- a/src/modules/load_mon/load_mon.cpp
+++ b/src/modules/load_mon/load_mon.cpp
@@ -47,9 +47,10 @@
 #include <px4_module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <systemlib/cpuload.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/task_stack_info.h>
-#include <uORB/uORB.h>
 
 #if defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_INSTRUMENTATION)
 #  error load_mon support requires CONFIG_SCHED_INSTRUMENTATION
@@ -105,14 +106,14 @@ private:
 	void _stack_usage();
 
 	int _stack_task_index{0};
-	orb_advert_t _task_stack_info_pub{nullptr};
+	uORB::PublicationQueued<task_stack_info_s> _task_stack_info_pub{ORB_ID(task_stack_info)};
 #endif
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::SYS_STCK_EN>) _param_sys_stck_en
 	)
 
-	orb_advert_t _cpuload_pub{nullptr};
+	uORB::Publication<cpuload_s>  _cpuload_pub{ORB_ID(cpuload)};
 
 	hrt_abstime _last_idle_time{0};
 	hrt_abstime _last_idle_time_sample{0};
@@ -192,17 +193,12 @@ void LoadMon::_cpuload()
 	_last_idle_time = total_runtime;
 	_last_idle_time_sample = hrt_absolute_time();
 
-	cpuload_s cpuload = {};
+	cpuload_s cpuload{};
 	cpuload.load = 1.0f - (float)interval_idletime / (float)interval;
 	cpuload.ram_usage = _ram_used();
 	cpuload.timestamp = hrt_absolute_time();
 
-	if (_cpuload_pub == nullptr) {
-		_cpuload_pub = orb_advertise(ORB_ID(cpuload), &cpuload);
-
-	} else {
-		orb_publish(ORB_ID(cpuload), _cpuload_pub, &cpuload);
-	}
+	_cpuload_pub.publish(cpuload);
 }
 
 float LoadMon::_ram_used()
@@ -284,12 +280,7 @@ void LoadMon::_stack_usage()
 			task_stack_info.stack_free = stack_free;
 			task_stack_info.timestamp = hrt_absolute_time();
 
-			if (_task_stack_info_pub == nullptr) {
-				_task_stack_info_pub = orb_advertise_queue(ORB_ID(task_stack_info), &task_stack_info, num_tasks_per_cycle);
-
-			} else {
-				orb_publish(ORB_ID(task_stack_info), _task_stack_info_pub, &task_stack_info);
-			}
+			_task_stack_info_pub.publish(task_stack_info);
 
 			/*
 			 * Found task low on stack, report and exit. Continue here in next cycle.