Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-23 18:13:49 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

boards: cuav/x25-evo: Replace core_heater module with multi-instance heater. (#26624)

Browse Source
This commit is contained in:
CUAV Chen
2026-03-04 05:07:24 +08:00
committed by GitHub
parent 2d69eaee74
commit 24197831e6
7 changed files with 14 additions and 576 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
boards/cuav/x25-evo/CMakeLists.txt
View File

@@ -31,5 +31,4 @@
#
############################################################################
add_subdirectory(core_heater)
add_subdirectory(pwm_voltage)

39
boards/cuav/x25-evo/core_heater/CMakeLists.txt
View File

@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2025 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__core_heater
MAIN core_heater
COMPILE_FLAGS
SRCS
core_heater.cpp
)

263
boards/cuav/x25-evo/core_heater/core_heater.cpp
View File

@@ -1,263 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2025 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 core_heater.cpp
*
*/
#include "core_heater.h"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_io_heater.h>
ModuleBase::Descriptor Core_Heater::desc{task_spawn, custom_command, print_usage};
# ifndef GPIO_CORE_HEATER_OUTPUT
# error "To use the heater driver, the board_config.h must define and initialize GPIO_CORE_HEATER_OUTPUT"
# endif
Core_Heater::Core_Heater() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
_heater_status_pub.advertise();
}
Core_Heater::~Core_Heater()
{
disable_core_heater();
}
int Core_Heater::custom_command(int argc, char *argv[])
{
// Check if the driver is running.
if (!is_running(desc)) {
PX4_INFO("not running");
return PX4_ERROR;
}
return print_usage("Unrecognized command.");
}
void Core_Heater::disable_core_heater()
{
// Reset heater to off state.
px4_arch_unconfiggpio(GPIO_CORE_HEATER_OUTPUT);
}
void Core_Heater::initialize_core_heater_io()
{
// Initialize heater to off state.
px4_arch_configgpio(GPIO_CORE_HEATER_OUTPUT);
}
void Core_Heater::core_heater_off()
{
CORE_HEATER_OUTPUT_EN(false);
}
void Core_Heater::core_heater_on()
{
CORE_HEATER_OUTPUT_EN(true);
}
bool Core_Heater::initialize_topics()
{
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
if (sensor_accel_sub.get().timestamp != 0 &&
sensor_accel_sub.get().device_id != 0 &&
PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
if (sensor_accel_sub.get().device_id == (uint32_t)_param_core_temp_id.get()) {
_sensor_accel_sub.ChangeInstance(i);
_sensor_device_id = sensor_accel_sub.get().device_id;
initialize_core_heater_io();
return true;
}
}
}
return false;
}
void Core_Heater::Run()
{
if (should_exit()) {
exit_and_cleanup(desc);
return;
}
update_params();
if (_sensor_device_id == 0) {
if (!initialize_topics()) {
// if sensor still not found try again in 1 second
ScheduleDelayed(1_s);
return;
}
}
sensor_accel_s sensor_accel;
float temperature_delta {0.f};
if (_core_heater_on) {
// Turn the heater off.
_core_heater_on = false;
core_heater_off();
ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
} else if (_sensor_accel_sub.update(&sensor_accel)) {
// Update the current IMU sensor temperature if valid.
if (PX4_ISFINITE(sensor_accel.temperature)) {
temperature_delta = _param_core_imu_temp.get() - sensor_accel.temperature;
_temperature_last = sensor_accel.temperature;
}
_proportional_value = temperature_delta * _param_core_imu_temp_p.get();
_integrator_value += temperature_delta * _param_core_imu_temp_i.get();
_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
_controller_time_on_usec = static_cast<int>((_param_core_imu_temp_ff.get() + _proportional_value +
_integrator_value) * static_cast<float>(_controller_period_usec));
_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
if (fabsf(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
_temperature_target_met = true;
} else {
_temperature_target_met = false;
}
_core_heater_on = true;
core_heater_on();
ScheduleDelayed(_controller_time_on_usec);
}
publish_status();
}
void Core_Heater::publish_status()
{
heater_status_s status{};
status.device_id = _sensor_device_id;
status.heater_on = _core_heater_on;
status.temperature_sensor = _temperature_last;
status.temperature_target = _param_core_imu_temp.get();
status.temperature_target_met = _temperature_target_met;
status.controller_period_usec = _controller_period_usec;
status.controller_time_on_usec = _controller_time_on_usec;
status.proportional_value = _proportional_value;
status.integrator_value = _integrator_value;
status.feed_forward_value = _param_core_imu_temp_ff.get();
status.mode = heater_status_s::MODE_GPIO;
status.timestamp = hrt_absolute_time();
_heater_status_pub.publish(status);
}
int Core_Heater::start()
{
// Exit the driver if the sensor ID does not match the desired sensor.
if (_param_core_temp_id.get() == 0) {
PX4_ERR("Valid CORE_TEMP_ID required");
request_stop();
return PX4_ERROR;
}
update_params(true);
ScheduleNow();
return PX4_OK;
}
int Core_Heater::task_spawn(int argc, char *argv[])
{
Core_Heater *core_heater = new Core_Heater();
if (!core_heater) {
PX4_ERR("driver allocation failed");
return PX4_ERROR;
}
desc.object.store(core_heater);
desc.task_id = task_id_is_work_queue;
core_heater->start();
return 0;
}
void Core_Heater::update_params(const bool force)
{
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
// update parameters from storage
ModuleParams::updateParams();
}
}
int Core_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.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("core_heater", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int core_heater_main(int argc, char *argv[])
{
return ModuleBase::main(Core_Heater::desc, argc, argv);
}

161
boards/cuav/x25-evo/core_heater/core_heater.h
View File

@@ -1,161 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2025 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 core_heater.h
*
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/heater_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <mathlib/mathlib.h>
using namespace time_literals;
#define CONTROLLER_PERIOD_DEFAULT 10000
#define TEMPERATURE_TARGET_THRESHOLD 2.5f
class Core_Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
{
public:
static Descriptor desc;
Core_Heater();
virtual ~Core_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.
* @param argv Pointer to the input argument array.
* @return Returns 0 iff successful, -1 otherwise.
*/
static int task_spawn(int argc, char *argv[]);
/**
* @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();
private:
/** Disables the heater (either by GPIO). */
void disable_core_heater();
/** Turns the heater on (either by GPIO). */
void core_heater_on();
/** Turns the heater off (either by GPIO). */
void core_heater_off();
void initialize();
/** Enables / configures the heater (either by GPIO). */
void initialize_core_heater_io();
/** @brief Called once to initialize uORB topics. */
bool initialize_topics();
void publish_status();
/** @brief Calculates the heater element on/off time and schedules the next cycle. */
void Run() override;
/**
* @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 = false);
/** Work queue struct for the scheduler. */
static struct work_s _work;
bool _core_heater_initialized = false;
bool _core_heater_on = false;
bool _temperature_target_met = false;
int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
int _controller_time_on_usec = 0;
float _integrator_value = 0.0f;
float _proportional_value = 0.0f;
uORB::Publication<heater_status_s> _heater_status_pub{ORB_ID(heater_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
uint32_t _sensor_device_id{0};
float _temperature_last{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::CORE_IMU_TEMP_FF>) _param_core_imu_temp_ff,
(ParamFloat<px4::params::CORE_IMU_TEMP_I>) _param_core_imu_temp_i,
(ParamFloat<px4::params::CORE_IMU_TEMP_P>) _param_core_imu_temp_p,
(ParamFloat<px4::params::CORE_IMU_TEMP>) _param_core_imu_temp,
(ParamInt<px4::params::CORE_TEMP_ID>) _param_core_temp_id
)
};

94
boards/cuav/x25-evo/core_heater/core_heater_params.c
View File

@@ -1,94 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2025 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 core_heater_params.c
* Core Heater parameters.
*
*/
/**
* Target IMU device ID to regulate temperature.
*
* @category system
* @group Sensors
*/
PARAM_DEFINE_INT32(CORE_TEMP_ID, 0);
/**
* Target IMU temperature.
*
* @category system
* @group Sensors
* @unit celcius
* @min 0
* @max 85.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(CORE_IMU_TEMP, 55.0f);
/**
* IMU heater controller feedforward value.
*
* @category system
* @group Sensors
* @unit %
* @min 0
* @max 1.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(CORE_IMU_TEMP_FF, 0.05f);
/**
* IMU heater controller integrator gain value.
*
* @category system
* @group Sensors
* @unit us/C
* @min 0
* @max 1.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(CORE_IMU_TEMP_I, 0.025f);
/**
* IMU heater controller proportional gain value.
*
* @category system
* @group Sensors
* @unit us/C
* @min 0
* @max 2.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(CORE_IMU_TEMP_P, 1.0f);

13
boards/cuav/x25-evo/init/rc.board_defaults
View File

@@ -20,15 +20,14 @@ param set-default USB_MAV_MODE 5
param set-default UAVCAN_SUB_GPS 1
param set-default UAVCAN_SUB_BAT 1
# Enable IMU thermal control
# IMU thermal control (multi-instance heater)
# HEATER1_IMU_ID: 2818058(IIM42652 SPI1)
# HEATER2_IMU_ID: 3014698(IIM42653 SPI5)
param set-default SENS_EN_THERMAL 1
param set-default SENS_IMU_TEMP 45
param set-default HEATER1_IMU_ID 2818058
# CUAV core board IMU thermal control
param set-default CORE_IMU_TEMP 45
param set-default CORE_TEMP_ID 3014698
core_heater start
param set-default HEATER1_TEMP 45
param set-default HEATER2_IMU_ID 3014698
param set-default HEATER2_TEMP 45
# CUAV pwm voltage 3.3V/5V switch
pwm_voltage_apply start

19
boards/cuav/x25-evo/src/board_config.h
View File

@@ -175,17 +175,14 @@
#define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15
#define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14
/* HEATER
* PWM in future
*/
// IMU BOARD HEATER
/* HEATER */
#define GPIO_HEATER_OUTPUT
#define HEATER_NUM 1
#define GPIO_HEATER1_OUTPUT /* PB10 T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define HEATER1_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
// CORE BOARD HEATER
#define GPIO_CORE_HEATER_OUTPUT /* PE6 T15CH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
#define CORE_HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_CORE_HEATER_OUTPUT, (on_true))
#define HEATER_NUM 2
#define GPIO_HEATER1_OUTPUT /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define HEATER1_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER1_OUTPUT, (on_true))
#define GPIO_HEATER2_OUTPUT /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
#define HEATER2_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER2_OUTPUT, (on_true))
/* PE7 is nARMED
* The GPIO will be set as input while not armed HW will have external HW Pull UP.
@@ -406,7 +403,7 @@
GPIO_CAN1_SILENT_S0, \
GPIO_CAN2_SILENT_S1, \
GPIO_HEATER1_OUTPUT, \
GPIO_CORE_HEATER_OUTPUT, \
GPIO_HEATER2_OUTPUT, \
GPIO_nPOWER_IN_A, \
GPIO_nPOWER_IN_B, \
GPIO_nPOWER_IN_C, \