From 59b31e3c5b3eb3f2a87ded131e12f0c0596e0815 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Mon, 15 Feb 2021 17:39:05 -0700 Subject: [PATCH] Minor cleanup/error checking, static_casts, print_status() additions, and formatting in the heater driver. --- src/drivers/drv_io_heater.h | 6 +- src/drivers/heater/heater.cpp | 290 +++++++++++++++-------------- src/drivers/heater/heater.h | 57 ++---- src/drivers/heater/heater_params.c | 12 ++ 4 files changed, 186 insertions(+), 179 deletions(-) diff --git a/src/drivers/drv_io_heater.h b/src/drivers/drv_io_heater.h index f946042a58..65146e391b 100644 --- a/src/drivers/drv_io_heater.h +++ b/src/drivers/drv_io_heater.h @@ -47,11 +47,11 @@ * ioctl() definitions */ -#define IO_HEATER_DEVICE_PATH "/dev/px4io" +#define IO_HEATER_DEVICE_PATH "/dev/px4io" -#define _IO_HEATER_BASE (0x2e00) +#define _IO_HEATER_BASE (0x2e00) -#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0) +#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0) /* ... to IOX_SET_VALUE + 8 */ diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 910fc4aac7..f73cbfca08 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -74,13 +74,11 @@ Heater::Heater() : #endif - // Initialize heater to off state heater_enable(); } Heater::~Heater() { - // Reset heater to off state heater_disable(); #ifdef HEATER_PX4IO @@ -88,46 +86,6 @@ Heater::~Heater() #endif } -void Heater::heater_enable() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); -#endif -#ifdef HEATER_GPIO - px4_arch_configgpio(GPIO_HEATER_OUTPUT); -#endif -} - -void Heater::heater_disable() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); -#endif -#ifdef HEATER_GPIO - px4_arch_configgpio(GPIO_HEATER_OUTPUT); -#endif -} - -void Heater::heater_on() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); -#endif -#ifdef HEATER_GPIO - px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); -#endif -} - -void Heater::heater_off() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); -#endif -#ifdef HEATER_GPIO - px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); -#endif -} - int Heater::custom_command(int argc, char *argv[]) { // Check if the driver is running. @@ -139,56 +97,51 @@ int Heater::custom_command(int argc, char *argv[]) return print_usage("Unrecognized command."); } -void Heater::Run() +uint32_t Heater::get_sensor_id() { - if (should_exit()) { - exit_and_cleanup(); - return; - } + return _sensor_accel.device_id; +} - if (_heater_on) { - // Turn the heater off. - heater_off(); - _heater_on = false; +void Heater::heater_disable() +{ + // Reset heater to off state. +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); +#endif +#ifdef HEATER_GPIO + px4_arch_configgpio(GPIO_HEATER_OUTPUT); +#endif +} - } else { - update_params(false); +void Heater::heater_enable() +{ + // Initialize heater to off state. +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); +#endif +#ifdef HEATER_GPIO + px4_arch_configgpio(GPIO_HEATER_OUTPUT); +#endif +} - _sensor_accel_sub.update(&_sensor_accel); +void Heater::heater_off() +{ +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); +#endif +#ifdef HEATER_GPIO + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); +#endif +} - // Obtain the current IMU sensor temperature. - _sensor_temperature = _sensor_accel.temperature; - - // Calculate the temperature delta between the setpoint and reported temperature. - float temperature_delta = _param_sens_imu_temp.get() - _sensor_temperature; - - // Modulate the heater time on with a feedforward/PI controller. - _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); - _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); - - // Constrain the integrator value to no more than 25% of the duty cycle. - _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); - - // Calculate the duty cycle. This is a value between 0 and 1. - float duty = _proportional_value + _integrator_value; - - _controller_time_on_usec = (int)(duty * (float)_controller_period_usec); - - // Constrain the heater time within the allowable duty cycle. - _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); - - // Turn the heater on. - _heater_on = true; - heater_on(); - } - - // Schedule the next cycle. - if (_heater_on) { - ScheduleDelayed(_controller_time_on_usec); - - } else { - ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); - } +void Heater::heater_on() +{ +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); +#endif +#ifdef HEATER_GPIO + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); +#endif } void Heater::initialize_topics() @@ -196,16 +149,21 @@ void Heater::initialize_topics() // Get the total number of accelerometer instances. uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel)); - // Check each instance for the correct ID. + // Get the total number of accelerometer instances and check each instance for the correct ID. for (uint8_t x = 0; x < number_of_imus; x++) { - _sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x}; + _sensor_accel.device_id = 0; - if (!_sensor_accel_sub.advertised()) { - continue; + while (_sensor_accel.device_id == 0) { + _sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x}; + + if (!_sensor_accel_sub.advertised()) { + px4_usleep(100); + continue; + } + + _sensor_accel_sub.copy(&_sensor_accel); } - _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()) { break; @@ -221,55 +179,22 @@ void Heater::initialize_topics() int Heater::print_status() { - PX4_INFO("Sensor ID: %d - Temperature: %3.3fC, Setpoint: %3.2fC, Heater State: %s", + float _feedforward_value = _param_sens_imu_temp_ff.get(); + + PX4_INFO("Sensor ID: %d,\tSetpoint: %3.2fC,\t Sensor Temperature: %3.2fC,\tDuty Cycle (usec): %d", _sensor_accel.device_id, - (double)_sensor_temperature, - (double)_param_sens_imu_temp.get(), - _heater_on ? "On" : "Off"); + static_cast(_param_sens_imu_temp.get()), + static_cast(_sensor_accel.temperature), + _controller_period_usec); + PX4_INFO("Feed Forward control effort: %3.2f%%,\tProportional control effort: %3.2f%%,\tIntegrator control effort: %3.3f%%,\t Heater cycle: %3.2f%%", + static_cast(_feedforward_value * 100), + static_cast(_proportional_value * 100), + static_cast(_integrator_value * 100), + static_cast(static_cast(_controller_time_on_usec) / static_cast(_controller_period_usec) * 100)); return PX4_OK; } -int Heater::start() -{ - update_params(true); - initialize_topics(); - - ScheduleNow(); - - return PX4_OK; -} - -int Heater::task_spawn(int argc, char *argv[]) -{ - Heater *heater = new Heater(); - - if (!heater) { - PX4_ERR("driver allocation failed"); - return PX4_ERROR; - } - - _object.store(heater); - _task_id = task_id_is_work_queue; - - heater->start(); - - return 0; -} - -void Heater::update_params(const bool force) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - ModuleParams::updateParams(); - } -} - int Heater::print_usage(const char *reason) { if (reason) { @@ -291,6 +216,97 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE return 0; } +void Heater::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + if (_heater_on) { + // Turn the heater off. + heater_off(); + _heater_on = false; + + } else { + update_params(false); + + _sensor_accel_sub.update(&_sensor_accel); + + float temperature_delta {0.f}; + + // Update the current IMU sensor temperature if valid. + if (!isnan(_sensor_accel.temperature)) { + temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature; + } + + _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); + _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); + + if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { + _integrator_value = 0.f; + } + + // Guard against integrator wind up. + _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); + + _controller_time_on_usec = static_cast((_param_sens_imu_temp_ff.get() + _proportional_value + + _integrator_value) * static_cast(_controller_period_usec)); + + _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); + + _heater_on = true; + heater_on(); + } + + // Schedule the next cycle. + if (_heater_on) { + ScheduleDelayed(_controller_time_on_usec); + + } else { + ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); + } +} + +int Heater::start() +{ + update_params(true); + initialize_topics(); + + // Allow sufficient time for all additional sensors and processes to start. + ScheduleDelayed(100000); + return PX4_OK; +} + +int Heater::task_spawn(int argc, char *argv[]) +{ + Heater *heater = new Heater(); + + if (!heater) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; + } + + _object.store(heater); + _task_id = task_id_is_work_queue; + + heater->start(); + return 0; +} + +void Heater::update_params(const bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + } +} + /** * Main entry point for the heater driver module */ diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 4e72a58b89..126287c452 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -104,6 +104,9 @@ public: */ int controller_period(char *argv[]); + /** @brief Returns the id of the target sensor. */ + uint32_t get_sensor_id(); + /** * @brief Sets and/or reports the heater controller integrator gain value. * @param argv Pointer to the input argument array. @@ -118,12 +121,6 @@ public: */ float proportional(char *argv[]); - /** - * @brief Reports the heater target sensor. - * @return Returns the id of the target sensor - */ - uint32_t sensor_id(); - /** * @brief Initiates the heater driver work queue, starts a new background task, * and fails if it is already running. @@ -146,18 +143,12 @@ public: protected: - /** - * @brief Called once to initialize uORB topics. - */ + /** @brief Called once to initialize uORB topics. */ void initialize_topics(); private: - /** - * @brief Calculates the heater element on/off time, carries out - * closed loop feedback and feedforward temperature control, - * and schedules the next cycle. - */ + /** @brief Calculates the heater element on/off time and schedules the next cycle. */ void Run() override; /** @@ -166,27 +157,19 @@ private: */ void update_params(const bool force = false); - /** - * @brief Enables / configures the heater (either by GPIO or PX4IO) - */ + /** Enables / configures the heater (either by GPIO or PX4IO). */ void heater_enable(); - /** - * @brief Disnables the heater (either by GPIO or PX4IO) - */ + /** Disnables the heater (either by GPIO or PX4IO). */ void heater_disable(); - /** - * @brief Turns the heater on (either by GPIO or PX4IO) - */ + /** Turns the heater on (either by GPIO or PX4IO). */ void heater_on(); - /** - * @brief Turns the heater off (either by GPIO or PX4IO) - */ + /** Turns the heater off (either by GPIO or PX4IO). */ void heater_off(); - /** Work queue struct for the RTOS scheduler. */ + /** Work queue struct for the scheduler. */ static struct work_s _work; /** File descriptor for PX4IO for heater ioctl's */ @@ -194,28 +177,24 @@ private: int _io_fd; #endif - int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT; - - int _controller_time_on_usec = 0; - bool _heater_on = false; - float _integrator_value = 0.0f; + int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT; + int _controller_time_on_usec = 0; + + float _integrator_value = 0.0f; + float _proportional_value = 0.0f; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - float _proportional_value = 0.0f; - uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)}; sensor_accel_s _sensor_accel{}; - float _sensor_temperature = 0.0f; - - /** @note Declare local parameters using defined parameters. */ DEFINE_PARAMETERS( + (ParamFloat) _param_sens_imu_temp_ff, (ParamFloat) _param_sens_imu_temp_i, (ParamFloat) _param_sens_imu_temp_p, - (ParamInt) _param_sens_temp_id, - (ParamFloat) _param_sens_imu_temp + (ParamInt) _param_sens_temp_id, + (ParamFloat) _param_sens_imu_temp ) }; diff --git a/src/drivers/heater/heater_params.c b/src/drivers/heater/heater_params.c index 8dfb5256f4..151f9f3eaf 100644 --- a/src/drivers/heater/heater_params.c +++ b/src/drivers/heater/heater_params.c @@ -60,6 +60,18 @@ PARAM_DEFINE_INT32(SENS_TEMP_ID, 0); */ PARAM_DEFINE_FLOAT(SENS_IMU_TEMP, 55.0f); +/** + * IMU heater controller feedforward value. + * + * @category system + * @group Sensors + * @unit % + * @min 0 + * @max 1.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(SENS_IMU_TEMP_FF, 0.05f); + /** * IMU heater controller integrator gain value. *