From e7666aa5d8f2da30292bf7302eac5624bd90b972 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Sat, 21 Jul 2018 16:16:31 -0600 Subject: [PATCH] Added an imu heater driver, formatted whitespace, standardized comments, and added doxy documentation. --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + src/drivers/boards/px4fmu-v4/board_config.h | 22 +- src/drivers/boards/px4fmu-v4/init.c | 22 +- src/drivers/boards/px4fmu-v5/board_config.h | 4 +- src/drivers/heater/CMakeLists.txt | 43 ++ src/drivers/heater/heater.cpp | 431 ++++++++++++++++++++ src/drivers/heater/heater.h | 271 ++++++++++++ src/drivers/heater/heater_params.c | 93 +++++ 9 files changed, 861 insertions(+), 32 deletions(-) create mode 100644 src/drivers/heater/CMakeLists.txt create mode 100644 src/drivers/heater/heater.cpp create mode 100644 src/drivers/heater/heater.h create mode 100644 src/drivers/heater/heater_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 71018dc073..3224008736 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -386,6 +386,12 @@ then fi fi +# Heater driver for temperature regulated IMUs. +if param compare SENS_EN_THERMAL 1 +then + heater start +fi + # Sensors on the PWM interface bank if param compare SENS_EN_LL40LS 1 then diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 0e2717f305..0dc3ad29af 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -18,6 +18,7 @@ set(config_module_list drivers/blinkm drivers/camera_trigger drivers/gps + drivers/heater drivers/irlock drivers/mkblctrl drivers/oreoled diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 076d46caea..599874828c 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -54,7 +54,6 @@ /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ - #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) @@ -94,15 +93,12 @@ #define GPIO_SPI1_CS_PORTE_PIN15 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) /* Define the Data Ready interrupts On SPI 1. */ - #define GPIO_DRDY_PORTD_PIN15 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) #define GPIO_DRDY_PORTC_PIN14 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) #define GPIO_DRDY_PORTE_PIN12 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) - /* Define the Chip Selects for SPI2. */ - #define GPIO_SPI2_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI2_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) @@ -112,23 +108,19 @@ * Define the ability to shut off off the sensor signals * by changing the signals to inputs. */ - #define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) /* SPI 1 bus off. */ - #define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) #define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) #define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) /* SPI 1 CS's off. */ - #define GPIO_SPI1_CS_OFF_PORTC_PIN2 _PIN_OFF(GPIO_SPI1_CS_PORTC_PIN2) #define GPIO_SPI1_CS_OFF_PORTC_PIN15 _PIN_OFF(GPIO_SPI1_CS_PORTC_PIN15) #define GPIO_SPI1_CS_OFF_PORTE_PIN15 _PIN_OFF(GPIO_SPI1_CS_PORTE_PIN15) /* SPI 1 DRDY's off. */ - #define GPIO_DRDY_OFF_PORTD_PIN15 _PIN_OFF(GPIO_DRDY_PORTD_PIN15) #define GPIO_DRDY_OFF_PORTC_PIN14 _PIN_OFF(GPIO_DRDY_PORTC_PIN14) #define GPIO_DRDY_OFF_PORTE_PIN12 _PIN_OFF(GPIO_DRDY_PORTE_PIN12) @@ -137,8 +129,6 @@ * N.B we do not have control over the SPI 2 buss powered devices * so the the ms5611 is not resetable. */ - - #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_RAMTRON 2 #define PX4_SPI_BUS_BARO PX4_SPI_BUS_RAMTRON @@ -165,7 +155,7 @@ */ #define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO, 3) -/* I2C busses */ +/* I2C busses. */ #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION @@ -176,8 +166,7 @@ #define PX4_I2C_OBDEV_BMP280 0x76 /** - * ADC channels - * + * ADC channels: * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver. */ #define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) @@ -189,7 +178,6 @@ #define ADC_RC_RSSI_CHANNEL 11 /* Define Battery 1 Voltage Divider and A per V. */ - #define BOARD_BATTERY1_V_DIV (13.653333333f) #define BOARD_BATTERY1_A_PER_V (36.367515152f) @@ -260,7 +248,6 @@ /** * USB OTG FS: - * * PA9 OTG_FS_VBUS VBUS sensing. */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) @@ -283,6 +270,7 @@ #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3) #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) + /* For R12, this signal is active high. */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define BOARD_INVERT_RC_INPUT(_invert_true, _na) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true) @@ -293,6 +281,10 @@ #define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) #define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) +/* Heater pins */ +#define GPIO_HEATER_INPUT (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6) +#define GPIO_HEATER_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) + /* Power switch controls */ #define SPEKTRUM_POWER(_on_true) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (!_on_true)) diff --git a/src/drivers/boards/px4fmu-v4/init.c b/src/drivers/boards/px4fmu-v4/init.c index 107c2cdf6b..6a99b5761e 100644 --- a/src/drivers/boards/px4fmu-v4/init.c +++ b/src/drivers/boards/px4fmu-v4/init.c @@ -195,7 +195,6 @@ __EXPORT void stm32_boardinitialize(void) { // Reset all PWM to Low outputs. - board_on_reset(-1); // Configure LEDs. @@ -231,15 +230,16 @@ stm32_boardinitialize(void) stm32_configgpio(GPIO_8266_RST); // Safety - led on in led driver. - stm32_configgpio(GPIO_BTN_SAFETY); stm32_configgpio(GPIO_RSSI_IN); stm32_configgpio(GPIO_PPM_IN); // Configure SPI all interfaces GPIO. - stm32_spiinitialize(PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_SENSORS); + // Configure heater GPIO. + stm32_configgpio(GPIO_HEATER_INPUT); + stm32_configgpio(GPIO_HEATER_OUTPUT); } /**************************************************************************** @@ -277,7 +277,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) // Run C++ ctors before we go any further. - up_cxxinitialize(); # if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) @@ -294,7 +293,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) param_init(); // Configure the DMA allocator. - if (board_dma_alloc_init() < 0) { message("DMA alloc FAILED"); } @@ -329,7 +327,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) */ // Using Battery Backed Up SRAM. - int filesizes[CONFIG_STM32_BBSRAM_FILES + 1] = BSRAM_FILE_SIZES; stm32_bbsraminitialize(BBSRAM_PATH, filesizes); @@ -367,11 +364,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) * Yes. So add one to the boot count - this will be reset after a successful * commit to SD. */ - int reboots = hardfault_increment_reboot("boot", false); /* Also end the misery for a user that holds for a key down on the console. */ - int bytesWaiting; ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting)); @@ -381,7 +376,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) * Since we can not commit the fault dump to disk. Display it * to the console. */ - hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n", @@ -395,7 +389,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) */ // Clear any key press that got us here. - static volatile bool dbgContinue = false; int c = '>'; @@ -451,7 +444,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) } // outer switch } // for - } // inner if } // outer if @@ -465,11 +457,12 @@ __EXPORT int board_app_initialize(uintptr_t arg) led_off(LED_BLUE); // Power up the sensors. - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - // Configure SPI-based devices. + // Power down the heater. + stm32_gpiowrite(GPIO_HEATER_OUTPUT, 0); + // Configure SPI-based devices. spi1 = stm32_spibus_initialize(1); if (!spi1) { @@ -489,7 +482,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) up_udelay(20); // Get the SPI port for the FRAM. - spi2 = stm32_spibus_initialize(2); if (!spi2) { @@ -511,8 +503,8 @@ __EXPORT int board_app_initialize(uintptr_t arg) SPI_SELECT(spi2, PX4_SPIDEV_BARO, false); #ifdef CONFIG_MMCSD - // First, get an instance of the SDIO interface. + // First, get an instance of the SDIO interface. sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { diff --git a/src/drivers/boards/px4fmu-v5/board_config.h b/src/drivers/boards/px4fmu-v5/board_config.h index 0279bd5b4b..9180f18452 100644 --- a/src/drivers/boards/px4fmu-v5/board_config.h +++ b/src/drivers/boards/px4fmu-v5/board_config.h @@ -347,7 +347,7 @@ /* HEATER * PWM in future */ -#define GPIO_HEATER /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) /* PWM Capture * @@ -737,7 +737,7 @@ GPIO_CAN1_SILENT_S0, \ GPIO_CAN2_SILENT_S1, \ GPIO_CAN3_SILENT_S2, \ - GPIO_HEATER, \ + GPIO_HEATER_OUTPUT, \ GPIO_nPOWER_IN_A, \ GPIO_nPOWER_IN_B, \ GPIO_nPOWER_IN_C, \ diff --git a/src/drivers/heater/CMakeLists.txt b/src/drivers/heater/CMakeLists.txt new file mode 100644 index 0000000000..19cce63461 --- /dev/null +++ b/src/drivers/heater/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__heater + MAIN heater + STACK_MAIN 1024 + COMPILE_FLAGS + SRCS + heater.cpp + DEPENDS + platforms__common + modules__uORB + ) diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp new file mode 100644 index 0000000000..7038061858 --- /dev/null +++ b/src/drivers/heater/heater.cpp @@ -0,0 +1,431 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file heater.cpp + * + * @author Mark Sauder + * @author Alex Klimaj + * @author Jake Dahl + */ + +#include "heater.h" + +#include +#include +#include + +#ifndef GPIO_HEATER_INPUT +#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) +{ + px4_arch_configgpio(GPIO_HEATER_OUTPUT); + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); +} + +Heater::~Heater() +{ + // Drive the heater GPIO pin low. + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); + + // Verify if GPIO is low, and if not, configure it as an input pulldown then reconfigure as an output. + if (px4_arch_gpioread(GPIO_HEATER_OUTPUT)) { + px4_arch_configgpio(GPIO_HEATER_INPUT); + px4_arch_configgpio(GPIO_HEATER_OUTPUT); + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); + } + + // Unsubscribe from uORB topics. + orb_unsubscribe(_params_sub); + orb_unsubscribe(_sensor_accel_sub); +} + +int Heater::controller_period(char *argv[]) +{ + if (argv[1]) { + _controller_period_usec = atoi(argv[1]); + } + + PX4_INFO("controller period (usec): %i", _controller_period_usec); + return _controller_period_usec; +} + +int Heater::custom_command(int argc, char *argv[]) +{ + // Check if the driver is running. + if (!is_running() && !_object) { + PX4_INFO("not running"); + return PX4_ERROR; + } + + const char *arg_v = argv[0]; + + // Display/Set the heater controller period value (usec). + if (strcmp(arg_v, "controller_period") == 0) { + return get_instance()->controller_period(argv); + } + + // Display the heater on duty cycle as a percent. + if (strcmp(arg_v, "duty_cycle") == 0) { + return get_instance()->duty_cycle(); + } + + // Display/Set the heater driver feed forward value. + if (strcmp(arg_v, "feed_forward") == 0) { + return get_instance()->feed_forward(argv); + } + + // Display/Set the heater driver integrator gain value. + if (strcmp(arg_v, "integrator") == 0) { + return get_instance()->integrator(argv); + } + + // Display/Set the heater driver proportional gain value. + if (strcmp(arg_v, "proportional") == 0) { + return get_instance()->proportional(argv); + } + + // Display the id of the sensor we are controlling temperature on. + if (strcmp(arg_v, "sensor_id") == 0) { + return get_instance()->sensor_id(); + } + + // Displays/Set the current IMU temperature setpoint. + if (strcmp(arg_v, "setpoint") == 0) { + return get_instance()->temperature_setpoint(argv); + } + + // Displays the IMU reported temperature. + if (strcmp(arg_v, "temp") == 0) { + return get_instance()->sensor_temperature(); + } + + get_instance()->print_usage("Unrecognized command."); + return PX4_OK; +} + +void Heater::cycle() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + if (_heater_on) { + // Turn the heater off. + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); + _heater_on = false; + + // Check if GPIO is stuck on, and if so, configure it as an input pulldown then reconfigure as an output. + if (px4_arch_gpioread(GPIO_HEATER_OUTPUT)) { + px4_arch_configgpio(GPIO_HEATER_INPUT); + px4_arch_configgpio(GPIO_HEATER_OUTPUT); + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); + } + + } else { + update_params(false); + + orb_update(ORB_ID(sensor_accel), _sensor_accel_sub, &_sensor_accel); + + // Obtain the current IMU sensor temperature. + _sensor_temperature = _sensor_accel.temperature; + + // Calculate the temperature delta between the setpoint and reported temperature. + float temperature_delta = _p_temperature_setpoint.get() - _sensor_temperature; + + // Modulate the heater time on with a feedforward/PI controller. + _proportional_value = temperature_delta * _p_proportional_gain.get(); + _integrator_value += temperature_delta * _p_integrator_gain.get(); + + // Constrain the integrator value to no more than 25% of the duty cycle. + _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); + + _controller_time_on_usec = (int)((_p_feed_forward_value.get() + _proportional_value + + _integrator_value) * (float)_controller_period_usec); + + // Constrain the heater time within the allowable duty cycle. + _controller_time_on_usec = math::constrain(_controller_period_usec, 0, _controller_time_on_usec); + + // Filter the duty cycle value over a ~2 second time constant. + _duty_cycle = (0.05f * ((float)_controller_time_on_usec / (float)_controller_period_usec)) + (0.95f * _duty_cycle); + + // Turn the heater on. + _heater_on = true; + + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); + } + + + // Schedule the next cycle. + if (_heater_on) { + work_queue(LPWORK, &_work, (worker_t)&Heater::cycle_trampoline, this, + USEC2TICK(_controller_time_on_usec)); + + } else { + work_queue(LPWORK, &_work, (worker_t)&Heater::cycle_trampoline, this, + USEC2TICK(_controller_period_usec - _controller_time_on_usec)); + } +} + +void Heater::cycle_trampoline(void *arg) +{ + Heater *obj = reinterpret_cast(arg); + obj->cycle(); +} + +float Heater::duty_cycle() +{ + PX4_INFO("Average duty cycle: %3.1f%%", (double)(_duty_cycle * 100.f)); + return _duty_cycle; +} + +float Heater::feed_forward(char *argv[]) +{ + if (argv[1]) { + _p_feed_forward_value.set(atof(argv[1])); + + } + + PX4_INFO("Feed forward value: %2.5f", (double)_p_feed_forward_value.get()); + return _p_feed_forward_value.get(); +} + +void Heater::initialize_topics() +{ + // Get the total number of accelerometer instances. + size_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); + + while (orb_update(ORB_ID(sensor_accel), _sensor_accel_sub, &_sensor_accel) != PX4_OK) { + usleep(200000); + } + + // 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)_p_sensor_id.get()) { + PX4_INFO("IMU sensor identified."); + break; + } + } + + PX4_INFO("Device ID: %d", _sensor_accel.device_id); + + // Exit the driver if the sensor ID does not match the desired sensor. + if (_sensor_accel.device_id != (uint32_t)_p_sensor_id.get()) { + request_stop(); + PX4_ERR("Could not identify IMU sensor."); + } +} + +void Heater::initialize_trampoline(void *arg) +{ + Heater *heater = new Heater(); + + if (!heater) { + PX4_ERR("Heater driver alloc failed"); + return; + } + + _object = heater; + heater->start(); +} + +float Heater::integrator(char *argv[]) +{ + if (argv[1]) { + _p_integrator_gain.set(atof(argv[1])); + } + + PX4_INFO("Integrator gain: %2.5f", (double)_p_integrator_gain.get()); + return _p_integrator_gain.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", + (double)_sensor_temperature, + (double)_p_temperature_setpoint.get(), + _heater_on ? "On" : "Off"); + + return PX4_OK; +} + +int Heater::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint. + +This task can be started at boot from the startup scripts by setting SENS_EN_THERMAL or via CLI. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("heater", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("controller_period", "Reports the heater driver cycle period value, (us), and sets it if supplied an argument."); + PRINT_MODULE_USAGE_COMMAND_DESCR("duty_cycle", "Reports the heater duty cycle (%)."); + PRINT_MODULE_USAGE_COMMAND_DESCR("feed_forward", "Sets the feedforward value if supplied an argument and reports the current value."); + PRINT_MODULE_USAGE_COMMAND_DESCR("integrator", "Sets the integrator gain value if supplied an argument and reports the current value."); + PRINT_MODULE_USAGE_COMMAND_DESCR("proportional", "Sets the proportional gain value if supplied an argument and reports the current value."); + PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_id", "Reports the current IMU the heater is temperature controlling."); + PRINT_MODULE_USAGE_COMMAND_DESCR("setpoint", "Reports the current IMU temperature."); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Starts the IMU heater driver as a background task"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Reports the current IMU temperature, temperature setpoint, and heater on/off status."); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stops the IMU heater driver."); + PRINT_MODULE_USAGE_COMMAND_DESCR("temp", "Reports the current IMU temperature."); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +float Heater::proportional(char *argv[]) +{ + if (argv[1]) { + _p_proportional_gain.set(atof(argv[1])); + } + + PX4_INFO("Proportional gain: %2.5f", (double)_p_proportional_gain.get()); + return _p_proportional_gain.get(); +} + +uint32_t Heater::sensor_id() +{ + PX4_INFO("Sensor ID: %d", _sensor_accel.device_id); + return _sensor_accel.device_id; +} + +float Heater::sensor_temperature() +{ + PX4_INFO("IMU temp: %3.3f", (double)_sensor_temperature); + return _sensor_temperature; +} + +int Heater::start() +{ + if (is_running()) { + PX4_INFO("Driver already running."); + return PX4_ERROR; + } + + update_params(true); + initialize_topics(); + + // Kick off the cycling. We can call it directly because we're already in the work queue context + cycle(); + + PX4_INFO("Driver started successfully."); + + return PX4_OK; +} + +int Heater::task_spawn(int argc, char *argv[]) +{ + int ret = work_queue(LPWORK, &_work, (worker_t)&Heater::initialize_trampoline, nullptr, 0); + + if (ret < 0) { + return ret; + } + + ret = wait_until_running(); + + if (ret < 0) { + return ret; + } + + _task_id = task_id_is_work_queue; + return 0; +} + +float Heater::temperature_setpoint(char *argv[]) +{ + if (argv[1]) { + _p_temperature_setpoint.set(atof(argv[1])); + } + + PX4_INFO("Target temp: %3.3f", (double)_p_temperature_setpoint.get()); + return _p_temperature_setpoint.get(); +} + +void Heater::update_params(const bool force) +{ + bool updated; + parameter_update_s param_update; + + orb_check(_params_sub, &updated); + + if (updated || force) { + ModuleParams::updateParams(); + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + } +} + + +/** + * Main entry point for the heater driver module + */ +int heater_main(int argc, char *argv[]) +{ + return Heater::main(argc, argv); +} \ No newline at end of file diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h new file mode 100644 index 0000000000..6f46c80bcc --- /dev/null +++ b/src/drivers/heater/heater.h @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file heater.h + * + * @author Mark Sauder + * @author Alex Klimaj + * @author Jake Dahl + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#define CONTROLLER_PERIOD_DEFAULT 100000 + +/** + * @brief IMU Heater Controller driver used to maintain consistent + * temparature at the IMU. + */ +extern "C" __EXPORT int heater_main(int argc, char *argv[]); + + +class Heater : public ModuleBase, public ModuleParams +{ +public: + Heater(); + + virtual ~Heater(); + + /** + * @see ModuleBase::custom_command(). + * @brief main Main entry point to the module that should be + * called directly from the module's main method. + * @param argc The input argument count. + * @param argv Pointer to the input argument array. + * @return Returns 0 iff successful, -1 otherwise. + */ + static int custom_command(int argc, char *argv[]); + + /** + * @see ModuleBase::print_usage(). + * @brief Prints the module usage to the nuttshell console. + * @param reason The requested reason for printing to console. + */ + static int print_usage(const char *reason = nullptr); + + /** + * @see ModuleBase::task_spawn(). + * @brief Initializes the class in the same context as the work queue + * and starts the background listener. + * @return Returns 0 iff successful, -1 otherwise. + */ + static int task_spawn(int argc, char *argv[]); + + /** + * @brief Sets and/or reports the heater controller time period value in microseconds. + * @return Returns 0 iff successful, -1 otherwise. + */ + int controller_period(char *argv[]); + + /** + * @brief Reports the average heater on duty cycle as a percent. + * @return Returns the average heater on cycle duty cycle as a percent. + */ + float duty_cycle(); + + /** + * @brief Sets and/or reports the heater controller feed fordward value. + * @param argv Pointer to the input argument array. + * @return Returns the heater feed forward value iff successful, 0.0f otherwise. + */ + float feed_forward(char *argv[]); + + /** + * @brief Sets and/or reports the heater controller integrator gain value. + * @param argv Pointer to the input argument array. + * @return Returns the heater integrator gain value iff successful, 0.0f otherwise. + */ + float integrator(char *argv[]); + + /** + * @brief Sets and/or reports the heater controller proportional gain value. + * @param argv Pointer to the input argument array. + * @return Returns the heater proportional gain value iff successful, 0.0f otherwise. + */ + 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. + * @return Returns 1 iff start was successful. + */ + int start(); + + /** + * @brief Reports curent status and diagnostic information about the heater driver. + * @return Returns 0 iff successful, -1 otherwise. + */ + int print_status(); + + /** + * @brief Reports the current heater temperature. + * @return Returns the current heater temperature value iff successful, -1.0f otherwise. + */ + float sensor_temperature(); + + /** + * @brief Sets and/or reports the heater target temperature. + * @return Returns the heater target temperature value iff successful, -1.0f otherwise. + */ + float temperature_setpoint(char *argv[]); + +protected: + + /** + * @brief Called once to initialize uORB topics. + */ + void initialize_topics(); + + /** + * @see ModuleBase::initialize_trampoline(). + * @brief Trampoline initialization. + * @param arg Pointer to the task startup arguments. + */ + static void initialize_trampoline(void *arg); + +private: + + /** + * @brief Checks for new commands and processes them. + */ + void process_commands(); + + /** + * @brief Trampoline for the work queue. + */ + static void cycle_trampoline(void *arg); + + /** + * @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); + + /** + * @brief Updates and checks for updated uORB parameters. + * @param force Boolean to determine if an update check should be forced. + */ + void update_params(const bool force = true); + + /** @var _command_ack_pub The command ackowledgement topic. */ + orb_advert_t _command_ack_pub = nullptr; + + /** @var _controller_period_usec The heater controller time period in microseconds.*/ + int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT; + + /** @var _controller_time_on_usec The heater time-on in microseconds.*/ + int _controller_time_on_usec = 0; + + /** @var _duty_cycle The heater time-on duty cycle value. */ + float _duty_cycle = 0.0f; + + /** @var _heater_on Indicator for the heater on/off status. */ + bool _heater_on = false; + + /** @var _integrator_value The heater controller integrator value. */ + float _integrator_value = 0.0f; + + /** @var Local member variable to store the parameter subscriptions. */ + int _params_sub; + + /** @var _proportional_value The heater controller proportional value. */ + float _proportional_value = 0.0f; + + /** @struct _sensor_accel Accelerometer struct to receive uORB accelerometer data. */ + struct sensor_accel_s _sensor_accel = {}; + + /** @var _sensor_accel_sub The accelerometer subtopic subscribed to.*/ + int _sensor_accel_sub = -1; + + /** @var _sensor_temperature The sensor's reported temperature. */ + float _sensor_temperature = 0.0f; + + /** @var _temperature_setpoint The heater controller temperature setpoint target. */ + float _temperature_setpoint; + + /** @struct _work Work Queue struct for the RTOS scheduler. */ + static struct work_s _work; + + /** @note Declare local parameters using defined parameters. */ + DEFINE_PARAMETERS( + /** @var _feed_forward The heater controller feedforward value. */ + (ParamFloat) _p_feed_forward_value, + + /** @var _integrator_gain The heater controller integrator gain value. */ + (ParamFloat) _p_integrator_gain, + + /** @var _proportional_gain The heater controller proportional gain value. */ + (ParamFloat) _p_proportional_gain, + + /** @var _p_sensor_id The ID of sensor to control temperature. */ + (ParamInt) _p_sensor_id, + + /** @var _p_temperature_setpoint The heater controller temperature setpoint parameter. */ + (ParamFloat) _p_temperature_setpoint + ) +}; diff --git a/src/drivers/heater/heater_params.c b/src/drivers/heater/heater_params.c new file mode 100644 index 0000000000..a32d372dc0 --- /dev/null +++ b/src/drivers/heater/heater_params.c @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file heater_params.c + * Heater parameters. + * + * @author Mark Sauder + * @author Alex Klimaj + * @author Jake Dahl + */ + +/** + * Target IMU device ID to regulate temperature. + * + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_TEMP_ID, 1442826); + +/** + * Target IMU temperature. + * + * @group Sensors + * @unit C + * @min 0 + * @max 85.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(SENS_IMU_TEMP, 55.0f); + +/** + * IMU heater controller feedforward value. + * + * @group Sensors + * @unit microseconds + * @min 0 + * @max 1.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(SENS_IMU_TEMP_FF, 0.25f); + +/** + * IMU heater controller integrator gain value. + * + * @group Sensors + * @unit microseconds/C + * @min 0 + * @max 1.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(SENS_IMU_TEMP_I, 0.025f); + + +/** + * IMU heater controller proportional gain value. + * + * @group Sensors + * @unit microseconds/C + * @min 0 + * @max 1.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(SENS_IMU_TEMP_P, 0.25f);