From ca5651bd8b9f13dc0815d801f74a618144366532 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Jun 2019 12:59:14 -0400 Subject: [PATCH] heater move to new WQ and uORB::Subscription --- src/drivers/heater/heater.cpp | 104 +++++++--------------------------- src/drivers/heater/heater.h | 37 +++--------- 2 files changed, 28 insertions(+), 113 deletions(-) diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 0815b17aaf..94e3d9ae8f 100644 --- a/src/drivers/heater/heater.cpp +++ b/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(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; - } - - ret = wait_until_running(); - - if (ret < 0) { - return ret; + if (!heater) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; } + _object.store(heater); _task_id = task_id_is_work_queue; + + heater->start(); + 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(¶m_update) || force) { ModuleParams::updateParams(); - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); } } - /** * Main entry point for the heater driver module */ diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 1546829c52..22425f7ce8 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -41,13 +41,13 @@ #pragma once -#include +#include #include #include #include #include -#include +#include #include #include @@ -62,7 +62,7 @@ extern "C" __EXPORT int heater_main(int argc, char *argv[]); -class Heater : public ModuleBase, public ModuleParams +class Heater : public ModuleBase, 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;