PX4 general work queue

.ci/Jenkinsfile-compile

@@ -43,7 +43,7 @@ pipeline {
           ]
 
           def nuttx_builds_other = [
-            target: ["px4_cannode-v1_default", "px4_esc-v1_default", "thiemar_s2740vc-v1_default"],
+            target: ["px4_esc-v1_default", "thiemar_s2740vc-v1_default"],
             image: docker_images.nuttx,
             archive: false
           ]

platforms/nuttx/src/px4_layer/CMakeLists.txt

@@ -44,6 +44,7 @@ if (NOT ${PX4_BOARD} MATCHES "px4_io")
 			nuttx_apps # up_cxxinitialize
 			nuttx_sched
 			drivers_boards_common_arch
+			px4_work_queue
 		)
 else()
 	add_library(px4_layer ${PX4_SOURCE_DIR}/src/platforms/empty.c)

platforms/nuttx/src/px4_layer/px4_init.cpp

@@ -37,6 +37,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
 #include <systemlib/cpuload.h>
 
 #include <fcntl.h>
@@ -104,5 +105,7 @@ int px4_platform_init(void)
 	cpuload_initialize_once();
 #endif
 
+	px4::WorkQueueManagerStart();
+
 	return PX4_OK;
 }

platforms/posix/CMakeLists.txt

@@ -62,10 +62,11 @@ else()
 			${df_driver_libs}
 			df_driver_framework
 			pthread m
-			
+
 			# horrible circular dependencies that need to be teased apart
-			px4_layer
-			px4_platform
+			px4_layer px4_platform
+			work_queue
+			parameters
 	)
 
 	if (NOT APPLE)

platforms/posix/src/px4_layer/CMakeLists.txt

@@ -54,7 +54,7 @@ add_library(px4_layer
 	${SHMEM_SRCS}
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
-target_link_libraries(px4_layer PRIVATE work_queue)
+target_link_libraries(px4_layer PRIVATE work_queue px4_work_queue)
 target_link_libraries(px4_layer PRIVATE px4_daemon)
 
 if(ENABLE_LOCKSTEP_SCHEDULER)

platforms/posix/src/px4_layer/px4_init.cpp

@@ -37,12 +37,15 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
 
 int px4_platform_init(void)
 {
 	hrt_init();
 
 	param_init();
 
+	px4::WorkQueueManagerStart();
+
 	return PX4_OK;
 }

platforms/qurt/src/px4_layer/px4_init.cpp

@@ -37,12 +37,15 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
 
 int px4_platform_init(void)
 {
 	hrt_init();
 
 	param_init();
 
+	px4::WorkQueueManagerStart();
+
 	return PX4_OK;
 }

posix-configs/SITL/init/test/cmd_template.in

@@ -25,7 +25,7 @@ mavlink start -x -u 14556 -r 2000000
 mavlink boot_complete
 
 @cmd_name@ start
-sleep 1
+sleep 3
 @cmd_name@ start
 sleep 1
 @cmd_name@ stop

src/drivers/barometer/bmp280/CMakeLists.txt

@@ -40,5 +40,7 @@ px4_add_module(
 		bmp280_spi.cpp
 		bmp280_i2c.cpp
 		bmp280.cpp
+	DEPENDS
+		px4_work_queue
 	)
 

src/drivers/barometer/bmp280/bmp280.cpp

@@ -54,7 +54,6 @@
 #include <px4_log.h>
 
 #include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
@@ -68,6 +67,7 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 
 enum BMP280_BUS {
@@ -78,15 +78,11 @@ enum BMP280_BUS {
 	BMP280_BUS_SPI_EXTERNAL
 };
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
 /*
  * BMP280 internal constants and data structures.
  */
 
-class BMP280 : public cdev::CDev
+class BMP280 : public cdev::CDev, public px4::ScheduledWorkItem
 {
 public:
 	BMP280(bmp280::IBMP280 *interface, const char *path);
@@ -109,9 +105,8 @@ class BMP280 : public cdev::CDev
 
 	uint8_t				_curr_ctrl;
 
-	struct work_s		_work;
-	unsigned			_report_ticks; // 0 - no cycling, otherwise period of sending a report
-	unsigned			_max_mesure_ticks; //ticks needed to measure
+	unsigned			_report_interval; // 0 - no cycling, otherwise period of sending a report
+	unsigned			_max_measure_interval;
 
 	ringbuffer::RingBuffer	*_reports;
 
@@ -135,8 +130,8 @@ class BMP280 : public cdev::CDev
 	/* periodic execution helpers */
 	void			start_cycle();
 	void			stop_cycle();
-	void			cycle(); //main execution
-	static void		cycle_trampoline(void *arg);
+
+	void			Run() override;
 
 	int		measure(); //start measure
 	int		collect(); //get results and publish
@@ -149,9 +144,10 @@ extern "C" __EXPORT int bmp280_main(int argc, char *argv[]);
 
 BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
 	CDev(path),
+	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
 	_interface(interface),
 	_running(false),
-	_report_ticks(0),
+	_report_interval(0),
 	_reports(nullptr),
 	_collect_phase(false),
 	_baro_topic(nullptr),
@@ -161,8 +157,6 @@ BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
 	_measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")),
 	_comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors"))
 {
-	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
 }
 
 BMP280::~BMP280()
@@ -226,7 +220,7 @@ BMP280::init()
 	/* set config, recommended settings */
 	_curr_ctrl = BPM280_CTRL_P16 | BPM280_CTRL_T2;
 	_interface->set_reg(_curr_ctrl, BPM280_ADDR_CTRL);
-	_max_mesure_ticks = USEC2TICK(BPM280_MT_INIT + BPM280_MT * (16 - 1 + 2 - 1));
+	_max_measure_interval = BPM280_MT_INIT + BPM280_MT * (16 - 1 + 2 - 1);
 	_interface->set_reg(BPM280_CONFIG_F16, BPM280_ADDR_CONFIG);
 
 	/* get calibration and pre process them*/
@@ -256,7 +250,7 @@ BMP280::init()
 		return -EIO;
 	}
 
-	usleep(TICK2USEC(_max_mesure_ticks));
+	usleep(_max_measure_interval);
 
 	if (collect()) {
 		return -EIO;
@@ -289,7 +283,7 @@ BMP280::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* if automatic measurement is enabled */
-	if (_report_ticks > 0) {
+	if (_report_interval > 0) {
 
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
@@ -315,7 +309,7 @@ BMP280::read(struct file *filp, char *buffer, size_t buflen)
 		return -EIO;
 	}
 
-	usleep(TICK2USEC(_max_mesure_ticks));
+	usleep(_max_measure_interval);
 
 	if (collect()) {
 		return -EIO;
@@ -335,31 +329,31 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case SENSORIOCSPOLLRATE: {
 
-			unsigned ticks = 0;
+			unsigned interval = 0;
 
 			switch (arg) {
 
 			case 0:
 				return -EINVAL;
 
 			case SENSOR_POLLRATE_DEFAULT:
-				ticks = _max_mesure_ticks;
+				interval = _max_measure_interval;
 
 			/* FALLTHROUGH */
 			default: {
-					if (ticks == 0) {
-						ticks = USEC2TICK(USEC_PER_SEC / arg);
+					if (interval == 0) {
+						interval = (USEC_PER_SEC / arg);
 					}
 
 					/* do we need to start internal polling? */
-					bool want_start = (_report_ticks == 0);
+					bool want_start = (_report_interval == 0);
 
 					/* check against maximum rate */
-					if (ticks < _max_mesure_ticks) {
+					if (interval < _max_measure_interval) {
 						return -EINVAL;
 					}
 
-					_report_ticks = ticks;
+					_report_interval = interval;
 
 					if (want_start) {
 						start_cycle();
@@ -389,41 +383,33 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 void
 BMP280::start_cycle()
 {
-
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_running = true;
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, 1);
+	ScheduleNow();
 }
 
 void
 BMP280::stop_cycle()
 {
 	_running = false;
-	work_cancel(HPWORK, &_work);
+	ScheduleClear();
 }
 
 void
-BMP280::cycle_trampoline(void *arg)
-{
-	BMP280 *dev = reinterpret_cast<BMP280 *>(arg);
-
-	dev->cycle();
-}
-
-void
-BMP280::cycle()
+BMP280::Run()
 {
 	if (_collect_phase) {
 		collect();
-		unsigned wait_gap = _report_ticks - _max_mesure_ticks;
+		unsigned wait_gap = _report_interval - _max_measure_interval;
 
 		if ((wait_gap != 0) && (_running)) {
-			work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this,
-				   wait_gap); //need to wait some time before new measurement
+			//need to wait some time before new measurement
+			ScheduleDelayed(wait_gap);
+
 			return;
 		}
 
@@ -432,9 +418,8 @@ BMP280::cycle()
 	measure();
 
 	if (_running) {
-		work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, _max_mesure_ticks);
+		ScheduleDelayed(_max_measure_interval);
 	}
-
 }
 
 int
@@ -517,7 +502,7 @@ BMP280::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-	printf("poll interval:  %u us \n", _report_ticks * USEC_PER_TICK);
+	printf("poll interval:  %u us \n", _report_interval);
 	_reports->print_info("report queue");
 
 	sensor_baro_s brp = {};

src/drivers/barometer/bmp280/bmp280.h

@@ -148,6 +148,8 @@ class IBMP280
 	// bulk read of calibration data into buffer, return same pointer
 	virtual bmp280::calibration_s *get_calibration(uint8_t addr) = 0;
 
+	virtual uint32_t get_device_id() const = 0;
+
 };
 
 } /* namespace */

src/drivers/barometer/bmp280/bmp280_i2c.cpp

@@ -60,6 +60,8 @@ class BMP280_I2C: public device::I2C, public bmp280::IBMP280
 	bmp280::data_s *get_data(uint8_t addr);
 	bmp280::calibration_s *get_calibration(uint8_t addr);
 
+	uint32_t get_device_id() const override { return device::I2C::get_device_id(); }
+
 private:
 	struct bmp280::calibration_s _cal;
 	struct bmp280::data_s _data;

src/drivers/barometer/bmp280/bmp280_spi.cpp

@@ -76,6 +76,8 @@ class BMP280_SPI: public device::SPI, public bmp280::IBMP280
 	bmp280::data_s *get_data(uint8_t addr);
 	bmp280::calibration_s *get_calibration(uint8_t addr);
 
+	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
+
 private:
 	spi_calibration_s _cal;
 	spi_data_s _data;