qmc5883 move to px4 work queue

src/drivers/magnetometer/qmc5883/CMakeLists.txt

@@ -40,4 +40,6 @@ px4_add_module(
 		qmc5883_i2c.cpp
 		qmc5883_spi.cpp
 		qmc5883.cpp
+	DEPENDS
+		px4_work_queue
 	)

src/drivers/magnetometer/qmc5883/qmc5883.cpp

@@ -56,7 +56,7 @@
 #include <unistd.h>
 
 #include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <nuttx/clock.h>
 
 #include <board_config.h>
@@ -134,11 +134,7 @@ enum QMC5883_BUS {
 	QMC5883_BUS_SPI
 };
 
-#ifndef CONFIG_SCHED_WORKQUEUE
+class QMC5883 : public device::CDev, public px4::ScheduledWorkItem
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-class QMC5883 : public device::CDev
 {
 public:
 	QMC5883(device::Device *interface, const char *path, enum Rotation rotation);
@@ -163,8 +159,7 @@ protected:
 	Device			*_interface;
 
 private:
-	work_s			_work{};
+	unsigned		_measure_interval{0};
-	unsigned		_measure_ticks;
 
 	ringbuffer::RingBuffer	*_reports;
 	struct mag_calibration_s	_scale;
@@ -228,15 +223,7 @@ private:
 	 * and measurement to provide the most recent measurement possible
 	 * at the next interval.
 	 */
-	void			cycle();
+	void			Run() override;
-
-	/**
-	 * Static trampoline from the workq context; because we don't have a
-	 * generic workq wrapper yet.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		cycle_trampoline(void *arg);
 
 	/**
 	 * Write a register.
@@ -274,8 +261,8 @@ extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]);
 
 QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
 	CDev("QMC5883", path),
+	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
 	_interface(interface),
-	_measure_ticks(0),
 	_reports(nullptr),
 	_scale{},
 	_range_scale(1.0f / 12000.0f),
@@ -301,9 +288,6 @@ QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rota
 	_device_id.devid_s.address = _interface->get_device_address();
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883;
 
-	// enable debug() calls
-	_debug_enabled = false;
-
 	// default scaling
 	_scale.x_offset = 0;
 	_scale.x_scale = 1.0f;
@@ -311,9 +295,6 @@ QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rota
 	_scale.y_scale = 1.0f;
 	_scale.z_offset = 0;
 	_scale.z_scale = 1.0f;
-
-	// work_cancel in the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
 }
 
 QMC5883::~QMC5883()
@@ -407,7 +388,7 @@ QMC5883::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* if automatic measurement is enabled */
-	if (_measure_ticks > 0) {
+	if (_measure_interval > 0) {
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
 		 * Note that we may be pre-empted by the workq thread while we are doing this;
@@ -462,10 +443,10 @@ QMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 			/* set default polling rate */
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+					bool want_start = (_measure_interval == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(QMC5883_CONVERSION_INTERVAL);
+					_measure_interval = QMC5883_CONVERSION_INTERVAL;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -478,18 +459,18 @@ QMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+					bool want_start = (_measure_interval == 0);
 
 					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+					unsigned interval = (1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(QMC5883_CONVERSION_INTERVAL)) {
+					if (interval < QMC5883_CONVERSION_INTERVAL) {
 						return -EINVAL;
 					}
 
 					/* update interval for next measurement */
-					_measure_ticks = ticks;
+					_measure_interval = interval;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -535,16 +516,16 @@ QMC5883::start()
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&QMC5883::cycle_trampoline, this, 1);
+	ScheduleNow();
 }
 
 void
 QMC5883::stop()
 {
-	if (_measure_ticks > 0) {
+	if (_measure_interval > 0) {
 		/* ensure no new items are queued while we cancel this one */
-		_measure_ticks = 0;
+		_measure_interval = 0;
-		work_cancel(HPWORK, &_work);
+		ScheduleClear();
 	}
 }
 
@@ -558,7 +539,6 @@ QMC5883::reset()
 	/* software reset */
 	write_reg(QMC5883_ADDR_CONTROL_2, QMC5883_SOFT_RESET);
 
-
 	/* set reset period to 0x01 */
 	write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT);
 
@@ -578,17 +558,9 @@ QMC5883::reset()
 }
 
 void
-QMC5883::cycle_trampoline(void *arg)
+QMC5883::Run()
 {
-	QMC5883 *dev = (QMC5883 *)arg;
+	if (_measure_interval == 0) {
-
-	dev->cycle();
-}
-
-void
-QMC5883::cycle()
-{
-	if (_measure_ticks == 0) {
 		return;
 	}
 
@@ -609,14 +581,9 @@ QMC5883::cycle()
 		/*
 		 * Is there a collect->measure gap?
 		 */
-		if (_measure_ticks > USEC2TICK(QMC5883_CONVERSION_INTERVAL)) {
+		if (_measure_interval > QMC5883_CONVERSION_INTERVAL) {
-
 			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
+			ScheduleDelayed(_measure_interval - QMC5883_CONVERSION_INTERVAL);
-				   &_work,
-				   (worker_t)&QMC5883::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(QMC5883_CONVERSION_INTERVAL));
 
 			return;
 		}
@@ -625,13 +592,9 @@ QMC5883::cycle()
 	/* next phase is collection */
 	_collect_phase = true;
 
-	if (_measure_ticks > 0) {
+	if (_measure_interval > 0) {
 		/* schedule a fresh cycle call when the measurement is done */
-		work_queue(HPWORK,
+		ScheduleDelayed(QMC5883_CONVERSION_INTERVAL);
-			   &_work,
-			   (worker_t)&QMC5883::cycle_trampoline,
-			   this,
-			   USEC2TICK(QMC5883_CONVERSION_INTERVAL));
 	}
 }
 
@@ -829,7 +792,7 @@ QMC5883::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("poll interval:  %u us\n", _measure_interval);
 	print_message(_last_report);
 	_reports->print_info("report queue");
 }