From 4d41dcaee2567ee561ad52c679e4adeddd98f019 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 22 Feb 2019 10:42:30 -0500
Subject: [PATCH] leddar_one move to px4 work queue

---
 .../distance_sensor/leddar_one/leddar_one.cpp | 28 ++++++++-----------
 1 file changed, 12 insertions(+), 16 deletions(-)

diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp
index 6b5ce4f493..51425a6d4a 100644
--- a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp
+++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include <px4_config.h>
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_getopt.h>
 #include <px4_defines.h>
 #include <px4_module.h>
@@ -109,20 +109,19 @@ struct __attribute__((__packed__)) reading_msg {
 	uint16_t crc; /* little-endian */
 };
 
-class leddar_one : public cdev::CDev
+class leddar_one : public cdev::CDev, public px4::ScheduledWorkItem
 {
 public:
 	leddar_one(const char *device_path, const char *serial_port, uint8_t rotation);
 	virtual ~leddar_one();
 
-	virtual int init();
+	virtual int init() override;
 	void start();
 	void stop();
 
 	virtual ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 
 private:
-	work_s _work;
 
 	int _fd = -1;
 	const char *_serial_port;
@@ -151,7 +150,7 @@ private:
 	void _publish(uint16_t distance_cm);
 	int _cycle();
 
-	static void cycle_trampoline(void *arg);
+	void Run() override;
 };
 
 extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]);
@@ -269,12 +268,12 @@ int leddar_one_main(int argc, char *argv[])
 
 leddar_one::leddar_one(const char *device_path, const char *serial_port, uint8_t rotation):
 	CDev(device_path),
+	ScheduledWorkItem(px4::wq_configurations::hp_default),
 	_rotation(rotation),
 	_collect_timeout_perf(perf_alloc(PC_COUNT, "leddar_one_collect_timeout")),
 	_comm_error(perf_alloc(PC_COUNT, "leddar_one_comm_errors")),
 	_sample_perf(perf_alloc(PC_ELAPSED, "leddar_one_sample"))
 {
-	memset(&_work, 0, sizeof(_work));
 	_serial_port = strdup(serial_port);
 }
 
@@ -394,7 +393,7 @@ int leddar_one::init()
 void
 leddar_one::stop()
 {
-	work_cancel(HPWORK, &_work);
+	ScheduleClear();
 }
 
 void leddar_one::start()
@@ -407,23 +406,20 @@ void leddar_one::start()
 	::close(_fd);
 	_fd = -1;
 
-	work_queue(HPWORK, &_work, (worker_t)&leddar_one::cycle_trampoline, this, USEC2TICK(WORK_USEC_INTERVAL));
+	ScheduleDelayed(WORK_USEC_INTERVAL);
 }
 
 void
-leddar_one::cycle_trampoline(void *arg)
+leddar_one::Run()
 {
-	leddar_one *dev = reinterpret_cast<leddar_one * >(arg);
-
-	if (dev->_fd != -1) {
-		dev->_cycle();
+	if (_fd != -1) {
+		_cycle();
 
 	} else {
-		dev->_fd_open();
+		_fd_open();
 	}
 
-	work_queue(HPWORK, &(dev->_work), (worker_t)&leddar_one::cycle_trampoline,
-		   dev, USEC2TICK(WORK_USEC_INTERVAL));
+	ScheduleDelayed(WORK_USEC_INTERVAL);
 }
 
 void leddar_one::_publish(uint16_t distance_mm)