ms5611 move to px4 work queue

src/drivers/barometer/ms5611/CMakeLists.txt

@@ -41,4 +41,6 @@ px4_add_module(
 		ms5611_spi.cpp
 		ms5611_i2c.cpp
 		ms5611.cpp
+	DEPENDS
+		px4_work_queue
 	)

src/drivers/barometer/ms5611/ms5611.cpp

@@ -33,7 +33,7 @@
 
 /**
  * @file ms5611.cpp
- * Driver for the MS5611 and MS6507 barometric pressure sensor connected via I2C or SPI.
+ * Driver for the MS5611 and MS5607 barometric pressure sensor connected via I2C or SPI.
  */
 
 #include <px4_config.h>
@@ -51,21 +51,16 @@
 #include <math.h>
 #include <unistd.h>
 
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
-
-#include <arch/board/board.h>
 #include <board_config.h>
 
 #include <drivers/device/device.h>
 #include <drivers/drv_baro.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/ringbuffer.h>
-
+#include <lib/perf/perf_counter.h>
-#include <perf/perf_counter.h>
 #include <systemlib/err.h>
 #include <platforms/px4_getopt.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 #include "ms5611.h"
 
@@ -83,10 +78,6 @@ enum MS5611_BUS {
 	MS5611_BUS_SPI_EXTERNAL
 };
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
 /* helper macro for handling report buffer indices */
 #define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
 
@@ -126,7 +117,7 @@ enum MS5611_BUS {
 #define MS5611_BARO_DEVICE_PATH_EXT	"/dev/ms5611_ext"
 #define MS5611_BARO_DEVICE_PATH_INT	"/dev/ms5611_int"
 
-class MS5611 : public cdev::CDev
+class MS5611 : public cdev::CDev, public px4::ScheduledWorkItem
 {
 public:
 	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type);
@@ -147,8 +138,7 @@ protected:
 
 	ms5611::prom_s		_prom;
 
-	struct work_s		_work {};
+	unsigned		_measure_interval{0};
-	unsigned		_measure_ticks;
 
 	ringbuffer::RingBuffer	*_reports;
 	enum MS56XX_DEVICE_TYPES _device_type;
@@ -173,17 +163,15 @@ protected:
 	/**
 	 * Initialize the automatic measurement state machine and start it.
 	 *
-	 * @param delay_ticks the number of queue ticks before executing the next cycle
-	 *
 	 * @note This function is called at open and error time.  It might make sense
 	 *       to make it more aggressive about resetting the bus in case of errors.
 	 */
-	void			start_cycle(unsigned delay_ticks = 1);
+	void			start();
 
 	/**
 	 * Stop the automatic measurement state machine.
 	 */
-	void			stop_cycle();
+	void			stop();
 
 	/**
 	 * Perform a poll cycle; collect from the previous measurement
@@ -198,15 +186,7 @@ protected:
 	 * 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);
 
 	/**
 	 * Issue a measurement command for the current state.
@@ -229,9 +209,9 @@ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path,
 	       enum MS56XX_DEVICE_TYPES device_type) :
 	CDev(path),
+	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
 	_interface(interface),
 	_prom(prom_buf.s),
-	_measure_ticks(0),
 	_reports(nullptr),
 	_device_type(device_type),
 	_collect_phase(false),
@@ -251,7 +231,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *
 MS5611::~MS5611()
 {
 	/* make sure we are truly inactive */
-	stop_cycle();
+	stop();
 
 	if (_class_instance != -1) {
 		unregister_class_devname(get_devname(), _class_instance);
@@ -400,7 +380,7 @@ MS5611::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.
@@ -474,14 +454,14 @@ MS5611::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(MS5611_CONVERSION_INTERVAL);
+					_measure_interval = MS5611_CONVERSION_INTERVAL;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
-						start_cycle();
+						start();
 					}
 
 					return OK;
@@ -490,22 +470,22 @@ MS5611::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 */
+					/* convert hz to interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+					unsigned interval = (1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
+					if (_measure_interval < MS5611_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) {
-						start_cycle();
+						start();
 					}
 
 					return OK;
@@ -530,34 +510,25 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 }
 
 void
-MS5611::start_cycle(unsigned delay_ticks)
+MS5611::start()
 {
-
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_measure_phase = 0;
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks);
+	ScheduleNow();
 }
 
 void
-MS5611::stop_cycle()
+MS5611::stop()
 {
-	work_cancel(HPWORK, &_work);
+	ScheduleClear();
 }
 
 void
-MS5611::cycle_trampoline(void *arg)
+MS5611::Run()
-{
-	MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
-
-	dev->cycle();
-}
-
-void
-MS5611::cycle()
 {
 	int ret;
 	unsigned dummy;
@@ -585,7 +556,7 @@ MS5611::cycle()
 			 * to wait 2.8 ms after issuing the sensor reset command
 			 * according to the MS5611 datasheet
 			 */
-			start_cycle(USEC2TICK(2800));
+			ScheduleDelayed(2800);
 			return;
 		}
 
@@ -598,14 +569,10 @@ MS5611::cycle()
 		 * doing pressure measurements at something close to the desired rate.
 		 */
 		if ((_measure_phase != 0) &&
-		    (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
+		    (_measure_interval > MS5611_CONVERSION_INTERVAL)) {
 
 			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
+			ScheduleDelayed(_measure_interval - MS5611_CONVERSION_INTERVAL);
-				   &_work,
-				   (worker_t)&MS5611::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
 
 			return;
 		}
@@ -618,7 +585,7 @@ MS5611::cycle()
 		/* issue a reset command to the sensor */
 		_interface->ioctl(IOCTL_RESET, dummy);
 		/* reset the collection state machine and try again */
-		start_cycle();
+		start();
 		return;
 	}
 
@@ -626,11 +593,7 @@ MS5611::cycle()
 	_collect_phase = true;
 
 	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
+	ScheduleDelayed(MS5611_CONVERSION_INTERVAL);
-		   &_work,
-		   (worker_t)&MS5611::cycle_trampoline,
-		   this,
-		   USEC2TICK(MS5611_CONVERSION_INTERVAL));
 }
 
 int
@@ -788,7 +751,7 @@ MS5611::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("poll interval:  %u\n", _measure_interval);
 	_reports->print_info("report queue");
 	printf("device:         %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");