From 574f7e9f007d6ebf015c5b9b6c2f720d8e62c35c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 4 Jun 2019 12:03:27 -0400
Subject: [PATCH] batt_smbus move to new WQ

---
 src/drivers/batt_smbus/batt_smbus.cpp | 17 ++++-------------
 src/drivers/batt_smbus/batt_smbus.h   | 10 +++-------
 2 files changed, 7 insertions(+), 20 deletions(-)

diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index 81a77e34f1..423be2ad07 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -48,9 +48,8 @@
 
 extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
 
-struct work_s BATT_SMBUS::_work = {};
-
 BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) :
+	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
 	_interface(interface),
 	_cycle(perf_alloc(PC_ELAPSED, "batt_smbus_cycle")),
 	_batt_topic(nullptr),
@@ -151,8 +150,7 @@ int BATT_SMBUS::task_spawn(int argc, char *argv[])
 				return PX4_ERROR;
 			}
 
-			// Throw it into the work queue.
-			work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, dev, 0);
+			dev->ScheduleNow();
 
 			return PX4_OK;
 
@@ -163,13 +161,7 @@ int BATT_SMBUS::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }
 
-void BATT_SMBUS::cycle_trampoline(void *arg)
-{
-	BATT_SMBUS *dev = (BATT_SMBUS *)arg;
-	dev->cycle();
-}
-
-void BATT_SMBUS::cycle()
+void BATT_SMBUS::Run()
 {
 	// Get the current time.
 	uint64_t now = hrt_absolute_time();
@@ -279,8 +271,7 @@ void BATT_SMBUS::cycle()
 		}
 
 		// Schedule a fresh cycle call when the measurement is done.
-		work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
-			   USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_US));
+		ScheduleDelayed(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
 	}
 }
 
diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h
index 36a857879f..d0a2ba5fe3 100644
--- a/src/drivers/batt_smbus/batt_smbus.h
+++ b/src/drivers/batt_smbus/batt_smbus.h
@@ -48,7 +48,7 @@
 #include <lib/drivers/smbus/SMBus.hpp>
 #include <mathlib/mathlib.h>
 #include <px4_config.h>
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <perf/perf_counter.h>
 #include <platforms/px4_module.h>
 #include <uORB/topics/battery_status.h>
@@ -144,7 +144,7 @@ int serial_number();
 
 
 
-class BATT_SMBUS : public ModuleBase<BATT_SMBUS>
+class BATT_SMBUS : public ModuleBase<BATT_SMBUS>, public px4::ScheduledWorkItem
 {
 public:
 
@@ -280,9 +280,7 @@ public:
 
 private:
 
-	static void cycle_trampoline(void *arg);
-
-	static work_s _work;
+	void Run() override;
 
 	perf_counter_t _cycle;
 
@@ -294,8 +292,6 @@ private:
 
 	bool _should_suspend{false};
 
-	void cycle();
-
 	/** @param _last_report Last published report, used for test(). */
 	battery_status_s _last_report = battery_status_s{};