ll40ls move to px4 work queue

src/drivers/distance_sensor/ll40ls/LidarLite.cpp

@@ -41,38 +41,6 @@
 
 #include "LidarLite.h"
 
-LidarLite::LidarLite() :
-	_min_distance(LL40LS_MIN_DISTANCE),
-	_max_distance(LL40LS_MAX_DISTANCE_V3),
-	_measure_ticks(0)
-{
-}
-
-void LidarLite::set_minimum_distance(const float min)
-{
-	_min_distance = min;
-}
-
-void LidarLite::set_maximum_distance(const float max)
-{
-	_max_distance = max;
-}
-
-float LidarLite::get_minimum_distance() const
-{
-	return _min_distance;
-}
-
-float LidarLite::get_maximum_distance() const
-{
-	return _max_distance;
-}
-
-uint32_t LidarLite::getMeasureTicks() const
-{
-	return _measure_ticks;
-}
-
 int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
@@ -87,10 +55,10 @@ int LidarLite::ioctl(device::file_t *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(LL40LS_CONVERSION_INTERVAL);
+					_measure_interval = (LL40LS_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -103,18 +71,18 @@ int LidarLite::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);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+					if (interval < (LL40LS_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) {

src/drivers/distance_sensor/ll40ls/LidarLite.h

@@ -57,8 +57,7 @@
 class LidarLite
 {
 public:
-	LidarLite();
-
+	LidarLite() = default;
 	virtual ~LidarLite() = default;
 
 	virtual int init() = 0;
@@ -89,12 +88,12 @@ protected:
 	* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
 	* and LL40LS_MAX_DISTANCE_V3
 	*/
-	void                set_minimum_distance(const float min);
-	void                set_maximum_distance(const float max);
-	float               get_minimum_distance() const;
-	float               get_maximum_distance() const;
+	void                set_minimum_distance(const float min) { _min_distance = min; }
+	void                set_maximum_distance(const float max) { _max_distance = max; }
+	float               get_minimum_distance() const { return _min_distance; }
+	float               get_maximum_distance() const { return _max_distance; }
 
-	uint32_t            getMeasureTicks() const;
+	uint32_t            getMeasureInterval() const { return _measure_interval; }
 
 	virtual int         measure() = 0;
 	virtual int         collect() = 0;
@@ -102,7 +101,7 @@ protected:
 	virtual int         reset_sensor() = 0;
 
 private:
-	float               _min_distance;
-	float               _max_distance;
-	uint32_t            _measure_ticks;
+	float               _min_distance{LL40LS_MIN_DISTANCE};
+	float               _max_distance{LL40LS_MAX_DISTANCE_V3};
+	uint32_t            _measure_interval{0};
 };

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -52,8 +52,8 @@
 
 LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) :
 	I2C("LL40LS", path, bus, address, 100000),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
 	_rotation(rotation),
-	_work{},
 	_reports(nullptr),
 	_sensor_ok(false),
 	_collect_phase(false),
@@ -74,9 +74,6 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr
 {
 	// up the retries since the device misses the first measure attempts
 	_retries = 3;
-
-	// work_cancel in the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
 }
 
 LidarLiteI2C::~LidarLiteI2C()
@@ -254,7 +251,7 @@ ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen)
 	}
 
 	/* if automatic measurement is enabled */
-	if (getMeasureTicks() > 0) {
+	if (getMeasureInterval() > 0) {
 
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
@@ -570,22 +567,15 @@ void LidarLiteI2C::start()
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1);
+	ScheduleNow();
 }
 
 void LidarLiteI2C::stop()
 {
-	work_cancel(HPWORK, &_work);
-}
-
-void LidarLiteI2C::cycle_trampoline(void *arg)
-{
-	LidarLiteI2C *dev = (LidarLiteI2C *)arg;
-
-	dev->cycle();
+	ScheduleClear();
 }
 
-void LidarLiteI2C::cycle()
+void LidarLiteI2C::Run()
 {
 	/* collection phase? */
 	if (_collect_phase) {
@@ -607,14 +597,10 @@ void LidarLiteI2C::cycle()
 			/*
 			 * Is there a collect->measure gap?
 			 */
-			if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+			if (getMeasureInterval() > (LL40LS_CONVERSION_INTERVAL)) {
 
 				/* schedule a fresh cycle call when we are ready to measure again */
-				work_queue(HPWORK,
-					   &_work,
-					   (worker_t)&LidarLiteI2C::cycle_trampoline,
-					   this,
-					   getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+				ScheduleDelayed(getMeasureInterval() - LL40LS_CONVERSION_INTERVAL);
 
 				return;
 			}
@@ -635,11 +621,7 @@ void LidarLiteI2C::cycle()
 	}
 
 	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&LidarLiteI2C::cycle_trampoline,
-		   this,
-		   USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+	ScheduleDelayed(LL40LS_CONVERSION_INTERVAL);
 }
 
 void LidarLiteI2C::print_info()
@@ -648,7 +630,7 @@ void LidarLiteI2C::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_sensor_resets);
 	perf_print_counter(_sensor_zero_resets);
-	printf("poll interval:  %u ticks\n", getMeasureTicks());
+	printf("poll interval:  %u \n", getMeasureInterval());
 	_reports->print_info("report queue");
 	printf("distance: %ucm (0x%04x)\n",
 	       (unsigned)_last_distance, (unsigned)_last_distance);

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -42,7 +42,7 @@
 
 #include "LidarLite.h"
 
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 #include <perf/perf_counter.h>
 
@@ -78,7 +78,7 @@
 #define LL40LS_PEAK_STRENGTH_LOW 135			// Minimum peak strength raw value for accepting a measurement
 #define LL40LS_PEAK_STRENGTH_HIGH 234			// Max peak strength raw value
 
-class LidarLiteI2C : public LidarLite, public device::I2C
+class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem
 {
 public:
 	LidarLiteI2C(int bus, const char *path,
@@ -113,7 +113,6 @@ protected:
 
 private:
 	uint8_t _rotation;
-	work_s              _work;
 	ringbuffer::RingBuffer          *_reports;
 	bool                _sensor_ok;
 	bool                _collect_phase;
@@ -173,17 +172,9 @@ private:
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
-	void                cycle();
+	void                Run() override;
 	int                 collect() 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);
-
 private:
 	LidarLiteI2C(const LidarLiteI2C &copy) = delete;
 	LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete;

src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp

@@ -51,8 +51,8 @@
 
 LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) :
 	CDev(path),
+	ScheduledWorkItem(px4::wq_configurations::hp_default),
 	_rotation(rotation),
-	_work{},
 	_reports(nullptr),
 	_class_instance(-1),
 	_orb_class_instance(-1),
@@ -122,7 +122,7 @@ void LidarLitePWM::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_read_errors);
 	perf_print_counter(_sensor_zero_resets);
-	PX4_INFO("poll interval:  %u ticks", getMeasureTicks());
+	PX4_INFO("poll interval:  %u ", getMeasureInterval());
 
 	print_message(_range);
 }
@@ -135,31 +135,20 @@ void LidarLitePWM::print_registers()
 void LidarLitePWM::start()
 {
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1);
+	ScheduleNow();
 }
 
 void LidarLitePWM::stop()
 {
-	work_cancel(HPWORK, &_work);
+	ScheduleClear();
 }
 
-void LidarLitePWM::cycle_trampoline(void *arg)
-{
-	LidarLitePWM *dev = (LidarLitePWM *)arg;
-
-	dev->cycle();
-}
-
-void LidarLitePWM::cycle()
+void LidarLitePWM::Run()
 {
 	measure();
 
 	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&LidarLitePWM::cycle_trampoline,
-		   this,
-		   getMeasureTicks());
+	ScheduleDelayed(getMeasureInterval());
 }
 
 int LidarLitePWM::measure()
@@ -213,7 +202,7 @@ ssize_t LidarLitePWM::read(device::file_t *filp, char *buffer, size_t buflen)
 	}
 
 	/* if automatic measurement is enabled */
-	if (getMeasureTicks() > 0) {
+	if (getMeasureInterval() > 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;

src/drivers/distance_sensor/ll40ls/LidarLitePWM.h

@@ -45,8 +45,7 @@
 
 #include "LidarLite.h"
 
-#include <px4_workqueue.h>
-
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 #include <drivers/device/ringbuffer.h>
 #include <perf/perf_counter.h>
@@ -57,7 +56,7 @@
 
 
 
-class LidarLitePWM : public LidarLite, public cdev::CDev
+class LidarLitePWM : public LidarLite, public cdev::CDev, public px4::ScheduledWorkItem
 {
 public:
 	LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
@@ -72,7 +71,7 @@ public:
 
 	void stop() override;
 
-	void cycle();
+	void Run() override;
 
 	/**
 	* @brief
@@ -86,14 +85,6 @@ public:
 	 */
 	void print_registers() 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);
-
 	const char *get_dev_name() override;
 
 protected:
@@ -108,7 +99,6 @@ protected:
 
 private:
 	uint8_t _rotation;
-	work_s			_work;
 	ringbuffer::RingBuffer	*_reports;
 	int			_class_instance;
 	int			_orb_class_instance;

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -51,10 +51,6 @@
 #include <stdio.h>
 #include <px4_getopt.h>
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
 #define LL40LS_DEVICE_PATH_PWM  "/dev/ll40ls_pwm"
 
 enum LL40LS_BUS {