yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
mcsauder
2018-07-21 16:16:31 -06:00
committed by Beat Küng
parent 039221fa93
commit e7666aa5d8
9 changed files with 861 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/drivers/boards/px4fmu-v4/board_config.h
View File

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

22
src/drivers/boards/px4fmu-v4/init.c
View File

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

4
src/drivers/boards/px4fmu-v5/board_config.h
View File

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

43
src/drivers/heater/CMakeLists.txt Normal file
View File

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

431
src/drivers/heater/heater.cpp Normal file
View File

@@ -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 <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);
}

271
src/drivers/heater/heater.h Normal file
View File

@@ -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 <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
)
};

93
src/drivers/heater/heater_params.c Normal file
View File

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