heater move to new WQ and uORB::Subscription

src/drivers/heater/heater.cpp

@@ -49,14 +49,12 @@
 #error "To use the heater driver, the board_config.h must define and initialize GPIO_HEATER_INPUT and GPIO_HEATER_OUTPUT"
 #endif
 
-struct work_s Heater::_work = {};
-
 Heater::Heater() :
-	ModuleParams(nullptr)
+	ModuleParams(nullptr),
+	ScheduledWorkItem(px4::wq_configurations::lp_default)
 {
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 }
 
 Heater::~Heater()
@@ -70,13 +68,6 @@ Heater::~Heater()
 		px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 		px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
 	}
-
-	// Unsubscribe from uORB topics.
-	orb_unsubscribe(_params_sub);
-
-	if (_sensor_accel_sub >= 0) {
-		orb_unsubscribe(_sensor_accel_sub);
-	}
 }
 
 int Heater::controller_period(char *argv[])
@@ -128,7 +119,7 @@ int Heater::custom_command(int argc, char *argv[])
 	return PX4_OK;
 }
 
-void Heater::cycle()
+void Heater::Run()
 {
 	if (should_exit()) {
 		exit_and_cleanup();
@@ -150,7 +141,7 @@ void Heater::cycle()
 	} else {
 		update_params(false);
 
-		orb_update(ORB_ID(sensor_accel), _sensor_accel_sub, &_sensor_accel);
+		_sensor_accel_sub.update(&_sensor_accel);
 
 		// Obtain the current IMU sensor temperature.
 		_sensor_temperature = _sensor_accel.temperature;
@@ -181,45 +172,33 @@ void Heater::cycle()
 
 	// Schedule the next cycle.
 	if (_heater_on) {
-		work_queue(LPWORK, &_work, (worker_t)&Heater::cycle_trampoline, this,
-			   USEC2TICK(_controller_time_on_usec));
+		ScheduleDelayed(_controller_time_on_usec);
 
 	} else {
-		work_queue(LPWORK, &_work, (worker_t)&Heater::cycle_trampoline, this,
-			   USEC2TICK(_controller_period_usec - _controller_time_on_usec));
+		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
 	}
 }
 
-void Heater::cycle_trampoline(void *argv)
-{
-	Heater *obj = reinterpret_cast<Heater *>(argv);
-	obj->cycle();
-}
-
 void Heater::initialize_topics()
 {
 	// Get the total number of accelerometer instances.
-	size_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
+	uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
 
 	// Check each instance for the correct ID.
-	for (size_t x = 0; x < number_of_imus; x++) {
-		_sensor_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), (int)x);
+	for (uint8_t x = 0; x < number_of_imus; x++) {
+		_sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x};
 
-		if (_sensor_accel_sub < 0) {
+		if (!_sensor_accel_sub.published()) {
 			continue;
 		}
 
-		while (orb_update(ORB_ID(sensor_accel), _sensor_accel_sub, &_sensor_accel) != PX4_OK) {
-			usleep(200000);
-		}
+		_sensor_accel_sub.copy(&_sensor_accel);
 
 		// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
 		if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) {
 			PX4_INFO("IMU sensor identified.");
 			break;
 		}
-
-		orb_unsubscribe(_sensor_accel_sub);
 	}
 
 	PX4_INFO("Device ID:  %d", _sensor_accel.device_id);
@@ -231,19 +210,6 @@ void Heater::initialize_topics()
 	}
 }
 
-void Heater::initialize_trampoline(void *argv)
-{
-	Heater *heater = new Heater();
-
-	if (!heater) {
-		PX4_ERR("driver allocation failed");
-		return;
-	}
-
-	_object.store(heater);
-	heater->start();
-}
-
 float Heater::integrator(char *argv[])
 {
 	if (argv[1]) {
@@ -254,26 +220,6 @@ float Heater::integrator(char *argv[])
 	return _param_sens_imu_temp_i.get();
 }
 
-int Heater::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
-{
-	bool newData = false;
-
-	// Check if there is new data to obtain.
-	if (orb_check(handle, &newData) != OK) {
-		return PX4_ERROR;
-	}
-
-	if (!newData) {
-		return PX4_ERROR;
-	}
-
-	if (orb_copy(meta, handle, buffer) != OK) {
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
 int Heater::print_status()
 {
 	PX4_INFO("Temperature: %3.3fC - Setpoint: %3.2fC - Heater State: %s",
@@ -339,8 +285,7 @@ int Heater::start()
 	update_params(true);
 	initialize_topics();
 
-	// Kick off the cycling. We can call it directly because we're already in the work queue context
-	cycle();
+	ScheduleNow();
 
 	PX4_INFO("Driver started successfully.");
 
@@ -349,19 +294,18 @@ int Heater::start()
 
 int Heater::task_spawn(int argc, char *argv[])
 {
-	int ret = work_queue(LPWORK, &_work, (worker_t)&Heater::initialize_trampoline, nullptr, 0);
+	Heater *heater = new Heater();
 
-	if (ret < 0) {
-		return ret;
+	if (!heater) {
+		PX4_ERR("driver allocation failed");
+		return PX4_ERROR;
 	}
 
-	ret = wait_until_running();
+	_object.store(heater);
+	_task_id = task_id_is_work_queue;
 
-	if (ret < 0) {
-		return ret;
-	}
+	heater->start();
 
-	_task_id = task_id_is_work_queue;
 	return 0;
 }
 
@@ -377,18 +321,12 @@ float Heater::temperature_setpoint(char *argv[])
 
 void Heater::update_params(const bool force)
 {
-	bool updated;
 	parameter_update_s param_update;
-
-	orb_check(_params_sub, &updated);
-
-	if (updated || force) {
+	if (_params_sub.update(&param_update) || force) {
 		ModuleParams::updateParams();
-		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
 	}
 }
 
-
 /**
  * Main entry point for the heater driver module
  */

src/drivers/heater/heater.h

@@ -41,13 +41,13 @@
 
 #pragma once
 
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_module.h>
 #include <px4_module_params.h>
 #include <px4_config.h>
 #include <px4_getopt.h>
 
-#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
 
@@ -62,7 +62,7 @@
 extern "C" __EXPORT int heater_main(int argc, char *argv[]);
 
 
-class Heater : public ModuleBase<Heater>, public ModuleParams
+class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	Heater();
@@ -149,36 +149,14 @@ protected:
 	 */
 	void initialize_topics();
 
-	/**
-	 * @see ModuleBase::initialize_trampoline().
-	 * @brief Trampoline initialization.
-	 * @param argv Pointer to the task startup arguments.
-	 */
-	static void initialize_trampoline(void *argv);
-
 private:
 
-	/**
-	 * @brief Trampoline for the work queue.
-	 * @param argv Pointer to the task startup arguments.
-	 */
-	static void cycle_trampoline(void *argv);
-
 	/**
 	 * @brief Calculates the heater element on/off time, carries out
 	 *        closed loop feedback and feedforward temperature control,
 	 *        and schedules the next cycle.
 	 */
-	void cycle();
-
-	/**
-	 * @brief Updates the uORB topics for local subscribers.
-	 * @param meta The uORB metadata to copy.
-	 * @param handle The uORB handle to obtain data from.
-	 * @param buffer The data buffer to copy data into.
-	 * @return Returns true iff update was successful.
-	 */
-	int orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+	void Run() override;
 
 	/**
 	 * @brief Updates and checks for updated uORB parameters.
@@ -197,13 +175,12 @@ private:
 
 	float _integrator_value = 0.0f;
 
-	int _params_sub = 0;
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 
 	float _proportional_value = 0.0f;
 
-	struct sensor_accel_s _sensor_accel = {};
-
-	int _sensor_accel_sub = -1;
+	uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
+	sensor_accel_s _sensor_accel{};
 
 	float _sensor_temperature = 0.0f;