vl53lxx move to px4 work queue

src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp

@@ -41,7 +41,7 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_getopt.h>
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 #include <drivers/device/i2c.h>
 
@@ -101,38 +101,33 @@
 #define VL53LXX_MAX_RANGING_DISTANCE 2.0f
 #define VL53LXX_MIN_RANGING_DISTANCE 0.0f
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-class VL53LXX : public device::I2C
+class VL53LXX : public device::I2C, public px4::ScheduledWorkItem
 {
 public:
 	VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
 		int bus = VL53LXX_BUS_DEFAULT, int address = VL53LXX_BASEADDR);
 
 	virtual ~VL53LXX();
 
-	virtual int init();
+	virtual int init() override;
 
-	virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
 
-	virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
+	virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
 
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
 	void print_info();
 
 protected:
-	virtual int probe();
+	virtual int probe() override;
 
 private:
 	uint8_t _rotation;
-	work_s _work;
 	ringbuffer::RingBuffer *_reports;
 	bool _sensor_ok;
-	int _measure_ticks;
+	int _measure_interval;
 	bool _collect_phase;
 	bool _new_measurement;
 	bool _measurement_started;
@@ -162,7 +157,7 @@ class VL53LXX : public device::I2C
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
-	void cycle();
+	void Run() override;
 	int measure();
 	int collect();
 
@@ -176,15 +171,6 @@ class VL53LXX : public device::I2C
 	bool spadCalculations();
 	bool sensorTuning();
 	bool singleRefCalibration(uint8_t byte);
-
-	/**
-	* 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);
-
 };
 
 
@@ -195,10 +181,11 @@ extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[]);
 
 VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
 	I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
 	_rotation(rotation),
 	_reports(nullptr),
 	_sensor_ok(false),
-	_measure_ticks(0),
+	_measure_interval(0),
 	_collect_phase(false),
 	_new_measurement(true),
 	_measurement_started(false),
@@ -211,12 +198,6 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
 {
 	// up the retries since the device misses the first measure attempts
 	I2C::_retries = 3;
-
-	// enable debug() calls
-	_debug_enabled = false;
-
-	// work_cancel in the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
 }
 
 VL53LXX::~VL53LXX()
@@ -351,10 +332,10 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 			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(VL53LXX_SAMPLE_RATE);
+					_measure_interval = (VL53LXX_SAMPLE_RATE);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -369,13 +350,13 @@ VL53LXX::ioctl(device::file_t *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);
 
 					/* 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) {
@@ -407,7 +388,7 @@ VL53LXX::read(device::file_t *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.
@@ -585,8 +566,8 @@ VL53LXX::measure()
 		readRegister(SYSRANGE_START_REG, system_start);
 
 		if ((system_start & 0x01) == 1) {
-			work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
-				   USEC2TICK(VL53LXX_US));		// reschedule every 1 ms until measurement is ready
+			ScheduleDelayed(VL53LXX_US);
+
 			ret = OK;
 			return ret;
 
@@ -601,8 +582,7 @@ VL53LXX::measure()
 		readRegister(SYSRANGE_START_REG, system_start);
 
 		if ((system_start & 0x01) == 1) {
-			work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
-				   USEC2TICK(VL53LXX_US));		// reschedule every 1 ms until measurement is ready
+			ScheduleDelayed(VL53LXX_US);
 			ret = OK;
 			return ret;
 
@@ -614,8 +594,7 @@ VL53LXX::measure()
 	readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement);
 
 	if ((wait_for_measurement & 0x07) == 0) {
-		work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
-			   USEC2TICK(VL53LXX_US));		// reschedule every 1 ms until measurement is ready
+		ScheduleDelayed(VL53LXX_US); // reschedule every 1 ms until measurement is ready
 		ret = OK;
 		return ret;
 	}
@@ -699,28 +678,17 @@ VL53LXX::start()
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_US));
+	ScheduleDelayed(VL53LXX_US);
 }
 
-
 void
 VL53LXX::stop()
 {
-	work_cancel(LPWORK, &_work);
+	ScheduleClear();
 }
 
-
 void
-VL53LXX::cycle_trampoline(void *arg)
-{
-	VL53LXX *dev = (VL53LXX *)arg;
-
-	dev->cycle();
-}
-
-
-void
-VL53LXX::cycle()
+VL53LXX::Run()
 {
 	measure();
 
@@ -731,26 +699,19 @@ VL53LXX::cycle()
 
 		collect();
 
-		work_queue(LPWORK,
-			   &_work,
-			   (worker_t)&VL53LXX::cycle_trampoline,
-			   this,
-			   _measure_ticks);
+		ScheduleDelayed(_measure_interval);
 	}
-
 }
 
-
 void
 VL53LXX::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");
 }
 
-
 bool
 VL53LXX::spadCalculations()
 {