mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 16:23:34 +08:00
Added an imu heater driver, formatted whitespace, standardized comments, and added doxy documentation.
This commit is contained in:
mcsauder
@@ -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, ¶m_update);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Main entry point for the heater driver module
|
||||
*/
|
||||
int heater_main(int argc, char *argv[])
|
||||
{
|
||||
return Heater::main(argc, argv);
|
||||
}
|
||||
Reference in New Issue
Block a user