Added an imu heater driver, formatted whitespace, standardized comments, and...

Added an imu heater driver, formatted whitespace, standardized comments, and added doxy documentation.

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

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

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))

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) {

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,                 \

src/drivers/heater/CMakeLists.txt

+############################################################################
+#
+#   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
+	)

src/drivers/heater/heater.cpp

+/****************************************************************************
+ *
+ *   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 <mcsauder@gmail.com>
+ * @author Alex Klimaj <alexklimaj@gmail.com>
+ * @author Jake Dahl <dahl.jakejacob@gmail.com>
+ */
+
+#include "heater.h"
+
+#include <px4_getopt.h>
+#include <px4_log.h>
+#include <drivers/drv_hrt.h>
+
+#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<Heater *>(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, &param_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

src/drivers/heater/heater.h

+/****************************************************************************
+ *
+ *   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 <mcsauder@gmail.com>
+ * @author Alex Klimaj <alexklimaj@gmail.com>
+ * @author Jake Dahl <dahl.jakejacob@gmail.com>
+ */
+
+#pragma once
+
+#include <string.h>
+#include <getopt.h>
+#include <parameters/param.h>
+
+#include <px4_workqueue.h>
+#include <px4_module.h>
+#include <px4_module_params.h>
+
+#include <px4_config.h>
+#include <px4_log.h>
+#include <px4_getopt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
+
+#include <mathlib/mathlib.h>
+
+#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<Heater>, 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<px4::params::SENS_IMU_TEMP_FF>)  _p_feed_forward_value,
+
+		/** @var _integrator_gain The heater controller integrator gain value. */
+		(ParamFloat<px4::params::SENS_IMU_TEMP_I>)  _p_integrator_gain,
+
+		/** @var _proportional_gain The heater controller proportional gain value. */
+		(ParamFloat<px4::params::SENS_IMU_TEMP_P>)  _p_proportional_gain,
+
+		/** @var _p_sensor_id The ID of sensor to control temperature. */
+		(ParamInt<px4::params::SENS_TEMP_ID>) _p_sensor_id,
+
+		/** @var _p_temperature_setpoint The heater controller temperature setpoint parameter. */
+		(ParamFloat<px4::params::SENS_IMU_TEMP>) _p_temperature_setpoint
+	)
+};

src/drivers/heater/heater_params.c

+/****************************************************************************
+ *
+ *   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 <mcsauder@gmail.com>
+ * @author Alex Klimaj <alexklimaj@gmail.com>
+ * @author Jake Dahl <dahl.jakejacob@gmail.com>
+ */
+
+/**
+ * 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);