From a0aee8f6c68cfb946f86cc213a0c18033d2d01b0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 19:53:44 +0100 Subject: [PATCH 01/11] ControlAllocator: introduce helicopter rotor rpm controller --- msg/CMakeLists.txt | 1 + msg/PwmInput.msg | 8 +- msg/RpmControlStatus.msg | 7 + .../ActuatorEffectivenessHelicopter.cpp | 7 +- .../ActuatorEffectivenessHelicopter.hpp | 4 + .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../ActuatorEffectiveness/RpmControl.hpp | 150 ++++++++++++++++++ .../control_allocator/ControlAllocator.cpp | 2 +- src/modules/control_allocator/module.yaml | 35 ++++ 9 files changed, 209 insertions(+), 7 deletions(-) create mode 100644 msg/RpmControlStatus.msg create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7abae7729370..d8d2393c0050 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,6 +181,7 @@ set(msg_files RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg + RpmControlStatus.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/PwmInput.msg b/msg/PwmInput.msg index fcc7dbe4accf..805d6fbd6bed 100644 --- a/msg/PwmInput.msg +++ b/msg/PwmInput.msg @@ -1,4 +1,4 @@ -uint64 timestamp # Time since system start (microseconds) -uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) -uint32 pulse_width # Pulse width, timer counts -uint32 period # Period, timer counts +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts (microseconds) +uint32 period # Period, timer counts (microseconds) diff --git a/msg/RpmControlStatus.msg b/msg/RpmControlStatus.msg new file mode 100644 index 000000000000..c1ed3dd99771 --- /dev/null +++ b/msg/RpmControlStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 rpm_raw # measured rpm +float32 rpm_estimate # filtered rpm +float32 rpm_setpoint # desired rpm + +float32 output diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b678c..76481da71b24 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector #include +#include "RpmControl.hpp" + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -131,4 +133,6 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe bool _main_motor_engaged{true}; const ActuatorType _tail_actuator_type; + + RpmControl _rpm_control{this}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2406b81bf80b..a203ac9821ee 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -69,6 +70,7 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D target_link_libraries(ActuatorEffectiveness PRIVATE mathlib + PID ) px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp new file mode 100644 index 000000000000..a5f413178e71 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 RpmControl.hpp + * + * Control rpm of a helicopter rotor. + * Input: PWM input pulse period from an rpm sensor + * Output: Duty cycle command for the ESC + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +using namespace time_literals; + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + ~RpmControl() = default; + + void setSpoolupProgress(float spoolup_progress) + { + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < .8f) { + _pid.resetIntegral(); + } + } + + float getActuatorCorrection() + { + if (_pwm_input_sub.updated()) { + pwm_input_s pwm_input{}; + + if (_pwm_input_sub.copy(&pwm_input)) { + if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 1.f); + + } else { + _rpm_raw = 0; + } + + _timestamp_last_rpm_measurement = pwm_input.timestamp; + } + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); + _timestamp_last_update = now; + + _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.update(_rpm_raw); + + const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s); + const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f; + + if (no_rpm_pulse_timeout && no_excessive_rpm) { + const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + + } else { + _pid.setGains(0.f, 0.f, 0.f); + } + + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + + float output = _pid.update(_rpm_filter.getState(), dt, true); + + rpm_control_status_s rpm_control_status{}; + rpm_control_status.rpm_raw = _rpm_raw; + rpm_control_status.rpm_estimate = _rpm_filter.getState(); + rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); + rpm_control_status.output = output; + rpm_control_status.timestamp = hrt_absolute_time(); + _rpm_control_status_pub.publish(rpm_control_status); + + // Publish estimated rpm for MAVLink -> UI in ground station + rpm_s rpm{ + .timestamp = hrt_absolute_time(), + .indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm + }; + + _rpm_pub.publish(rpm); + + return output; + } + +private: + uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; + uORB::Publication _rpm_pub {ORB_ID::rpm}; + + float _rpm_raw{0.f}; + float _spoolup_progress{0.f}; + AlphaFilter _rpm_filter; + PID _pid; + hrt_abstime _timestamp_last_update{0}; + hrt_abstime _timestamp_last_rpm_measurement{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_heli_rpm_sp, + (ParamFloat) _param_ca_heli_rpm_p, + (ParamFloat) _param_ca_heli_rpm_i + ) +}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e38..0bc3f9c86e9d 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -314,7 +314,7 @@ ControlAllocator::Run() #endif // Check if parameters have changed - if (_parameter_update_sub.updated() && !_armed) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df403..013b954033e6 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -528,6 +528,41 @@ parameters: which is mostly the case when the main rotor turns counter-clockwise. type: boolean default: 0 + CA_HELI_RPM_SP: + description: + short: Setpoint for main rotor rpm + long: | + Requires rpm feedback for the controller. + type: float + decimal: 0 + increment: 1 + min: 100 + max: 10000 + default: 1500 + CA_HELI_RPM_P: + description: + short: Proportional gain for rpm control + long: | + Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it. + + motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000 + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 + CA_HELI_RPM_I: + description: + short: Integral gain for rpm control + long: | + Same definition as the proportional gain but for integral. + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 # Others CA_FAILURE_MODE: From ddae928a0fef1657528a520cb5a47bbc6e45e3a2 Mon Sep 17 00:00:00 2001 From: oravla5 Date: Wed, 11 Dec 2024 18:55:57 +0100 Subject: [PATCH 02/11] RpmControl: class clean up --- .../ActuatorEffectiveness/RpmControl.hpp | 53 +++++++------------ 1 file changed, 18 insertions(+), 35 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index a5f413178e71..fb509a571519 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -72,33 +72,24 @@ class RpmControl : public ModuleParams float getActuatorCorrection() { - if (_pwm_input_sub.updated()) { - pwm_input_s pwm_input{}; + if (_rpm_sub.updated()) { + rpm_s rpm_input{}; - if (_pwm_input_sub.copy(&pwm_input)) { - if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) { - // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 1.f); - - } else { - _rpm_raw = 0; - } - - _timestamp_last_rpm_measurement = pwm_input.timestamp; + if (_rpm_sub.copy(&rpm_input)) { + _rpm_estimate = rpm_input.rpm_estimate; + _rpm_raw = rpm_input.rpm_raw; + _timestamp_last_rpm_measurement = rpm_input.timestamp; } } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); - _timestamp_last_update = now; + hrt_abstime current_time = hrt_absolute_time(); + const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = current_time; - _rpm_filter.setParameters(dt, 0.5f); - _rpm_filter.update(_rpm_raw); + const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s; + const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; - const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s); - const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f; - - if (no_rpm_pulse_timeout && no_excessive_rpm) { + if (rpm_measurement_timeout && no_excessive_rpm) { const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); @@ -109,38 +100,30 @@ class RpmControl : public ModuleParams _pid.setOutputLimit(.5f); _pid.setIntegralLimit(.5f); - float output = _pid.update(_rpm_filter.getState(), dt, true); + float output = _pid.update(_rpm_estimate, dt, true); rpm_control_status_s rpm_control_status{}; rpm_control_status.rpm_raw = _rpm_raw; - rpm_control_status.rpm_estimate = _rpm_filter.getState(); + rpm_control_status.rpm_estimate = _rpm_estimate;; rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); rpm_control_status.output = output; rpm_control_status.timestamp = hrt_absolute_time(); _rpm_control_status_pub.publish(rpm_control_status); - // Publish estimated rpm for MAVLink -> UI in ground station - rpm_s rpm{ - .timestamp = hrt_absolute_time(), - .indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm - }; - - _rpm_pub.publish(rpm); - return output; } private: - uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + static constexpr float RPM_MAX_VALUE = 1800.f; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; - uORB::Publication _rpm_pub {ORB_ID::rpm}; + float _rpm_estimate{0.f}; float _rpm_raw{0.f}; float _spoolup_progress{0.f}; - AlphaFilter _rpm_filter; PID _pid; - hrt_abstime _timestamp_last_update{0}; hrt_abstime _timestamp_last_rpm_measurement{0}; + hrt_abstime _timestamp_last_update{0}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_heli_rpm_sp, From 1ab7c764765e2b885bb68dead8ecfe4646be5302 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:18:51 +0100 Subject: [PATCH 03/11] RpmControl: name current timestamp now following the convention --- .../ActuatorEffectiveness/RpmControl.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index fb509a571519..c4485c57ecad 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -82,11 +82,11 @@ class RpmControl : public ModuleParams } } - hrt_abstime current_time = hrt_absolute_time(); - const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = current_time; + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = now; - const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s; + const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; if (rpm_measurement_timeout && no_excessive_rpm) { From d40c75625c067b1ab022aef63848f69dd1c4eb90 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:19:53 +0100 Subject: [PATCH 04/11] RpmControl: call local message instance after message name following the convention --- .../ActuatorEffectiveness/RpmControl.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index c4485c57ecad..9bdbf562b2da 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -73,12 +73,12 @@ class RpmControl : public ModuleParams float getActuatorCorrection() { if (_rpm_sub.updated()) { - rpm_s rpm_input{}; + rpm_s rpm{}; - if (_rpm_sub.copy(&rpm_input)) { - _rpm_estimate = rpm_input.rpm_estimate; - _rpm_raw = rpm_input.rpm_raw; - _timestamp_last_rpm_measurement = rpm_input.timestamp; + if (_rpm_sub.copy(&rpm)) { + _rpm_estimate = rpm.rpm_estimate; + _rpm_raw = rpm.rpm_raw; + _timestamp_last_rpm_measurement = rpm.timestamp; } } From 57a89aaf226e4daea517faef05b3dfa4107ce860 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:27:59 +0100 Subject: [PATCH 05/11] RpmControl: remove status message because it by now only contains redundant information --- msg/CMakeLists.txt | 1 - msg/RpmControlStatus.msg | 7 ------- .../ActuatorEffectiveness/RpmControl.hpp | 12 ------------ 3 files changed, 20 deletions(-) delete mode 100644 msg/RpmControlStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d8d2393c0050..7abae7729370 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,7 +181,6 @@ set(msg_files RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg - RpmControlStatus.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/RpmControlStatus.msg b/msg/RpmControlStatus.msg deleted file mode 100644 index c1ed3dd99771..000000000000 --- a/msg/RpmControlStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 rpm_raw # measured rpm -float32 rpm_estimate # filtered rpm -float32 rpm_setpoint # desired rpm - -float32 output diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 9bdbf562b2da..9af733995d6d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -50,7 +50,6 @@ #include #include #include -#include using namespace time_literals; @@ -77,7 +76,6 @@ class RpmControl : public ModuleParams if (_rpm_sub.copy(&rpm)) { _rpm_estimate = rpm.rpm_estimate; - _rpm_raw = rpm.rpm_raw; _timestamp_last_rpm_measurement = rpm.timestamp; } } @@ -102,24 +100,14 @@ class RpmControl : public ModuleParams float output = _pid.update(_rpm_estimate, dt, true); - rpm_control_status_s rpm_control_status{}; - rpm_control_status.rpm_raw = _rpm_raw; - rpm_control_status.rpm_estimate = _rpm_estimate;; - rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); - rpm_control_status.output = output; - rpm_control_status.timestamp = hrt_absolute_time(); - _rpm_control_status_pub.publish(rpm_control_status); - return output; } private: static constexpr float RPM_MAX_VALUE = 1800.f; uORB::Subscription _rpm_sub{ORB_ID(rpm)}; - uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; float _rpm_estimate{0.f}; - float _rpm_raw{0.f}; float _spoolup_progress{0.f}; PID _pid; hrt_abstime _timestamp_last_rpm_measurement{0}; From 73a8c5556c9cbaa3ed07a0f08c35de0aa192e3f2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:17:12 +0100 Subject: [PATCH 06/11] RpmControl: maximum rpm outliers are now caught by RpmCapture --- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 9af733995d6d..396183382f0d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -85,9 +85,8 @@ class RpmControl : public ModuleParams _timestamp_last_update = now; const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; - if (rpm_measurement_timeout && no_excessive_rpm) { + if (rpm_measurement_timeout) { const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); @@ -104,7 +103,6 @@ class RpmControl : public ModuleParams } private: - static constexpr float RPM_MAX_VALUE = 1800.f; uORB::Subscription _rpm_sub{ORB_ID(rpm)}; float _rpm_estimate{0.f}; From 86c8af95ab16af8fe3272a70d2a01bccda3d8de1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:30:43 +0100 Subject: [PATCH 07/11] RpmControl: split into cpp source file fixing includes --- .../ActuatorEffectiveness/CMakeLists.txt | 1 + .../ActuatorEffectiveness/RpmControl.cpp | 83 +++++++++++++++++++ .../ActuatorEffectiveness/RpmControl.hpp | 52 +----------- 3 files changed, 88 insertions(+), 48 deletions(-) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index a203ac9821ee..10ff984b2917 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.cpp RpmControl.hpp ) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 000000000000..56bccfe7795e --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#include "RpmControl.hpp" + +#include + +using namespace time_literals; + +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + +void RpmControl::setSpoolupProgress(float spoolup_progress) +{ + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < .8f) { + _pid.resetIntegral(); + } +} + +float RpmControl::getActuatorCorrection() +{ + if (_rpm_sub.updated()) { + rpm_s rpm{}; + + if (_rpm_sub.copy(&rpm)) { + _rpm_estimate = rpm.rpm_estimate; + _timestamp_last_rpm_measurement = rpm.timestamp; + } + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = now; + + const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; + + if (rpm_measurement_timeout) { + const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + + } else { + _pid.setGains(0.f, 0.f, 0.f); + } + + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + + float output = _pid.update(_rpm_estimate, dt, true); + + return output; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 396183382f0d..b5930902dba8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -43,64 +43,20 @@ #pragma once -#include #include - +#include #include #include -#include #include -using namespace time_literals; - class RpmControl : public ModuleParams { public: - RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + RpmControl(ModuleParams *parent); ~RpmControl() = default; - void setSpoolupProgress(float spoolup_progress) - { - _spoolup_progress = spoolup_progress; - _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); - - if (_spoolup_progress < .8f) { - _pid.resetIntegral(); - } - } - - float getActuatorCorrection() - { - if (_rpm_sub.updated()) { - rpm_s rpm{}; - - if (_rpm_sub.copy(&rpm)) { - _rpm_estimate = rpm.rpm_estimate; - _timestamp_last_rpm_measurement = rpm.timestamp; - } - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = now; - - const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - - if (rpm_measurement_timeout) { - const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; - _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); - - } else { - _pid.setGains(0.f, 0.f, 0.f); - } - - _pid.setOutputLimit(.5f); - _pid.setIntegralLimit(.5f); - - float output = _pid.update(_rpm_estimate, dt, true); - - return output; - } + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); private: uORB::Subscription _rpm_sub{ORB_ID(rpm)}; From a201a4e0aa3b1cc3a19346a22862ef8ec2dec358 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 18:39:15 +0100 Subject: [PATCH 08/11] RpmControl: simplify the entire control logic --- .../ActuatorEffectiveness/RpmControl.cpp | 42 +++++++++---------- .../ActuatorEffectiveness/RpmControl.hpp | 11 ++--- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index 56bccfe7795e..f61a392ad492 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -37,47 +37,45 @@ using namespace time_literals; -RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pid.setOutputLimit(PID_OUTPUT_LIMIT); + _pid.setIntegralLimit(PID_OUTPUT_LIMIT); +}; void RpmControl::setSpoolupProgress(float spoolup_progress) { _spoolup_progress = spoolup_progress; _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); - if (_spoolup_progress < .8f) { + if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) { _pid.resetIntegral(); } } float RpmControl::getActuatorCorrection() { + hrt_abstime now = hrt_absolute_time(); + + // RPM measurement update if (_rpm_sub.updated()) { rpm_s rpm{}; if (_rpm_sub.copy(&rpm)) { - _rpm_estimate = rpm.rpm_estimate; - _timestamp_last_rpm_measurement = rpm.timestamp; + const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f); + _timestamp_last_measurement = rpm.timestamp; + + const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); } } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = now; - - const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - - if (rpm_measurement_timeout) { - const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; - _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); - - } else { - _pid.setGains(0.f, 0.f, 0.f); + // Timeout + if (now > _timestamp_last_measurement + 1_s) { + _pid.resetIntegral(); + _actuator_correction = 0.f; } - _pid.setOutputLimit(.5f); - _pid.setIntegralLimit(.5f); - - float output = _pid.update(_rpm_estimate, dt, true); - - return output; + return _actuator_correction; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b5930902dba8..b412445048bd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -59,13 +59,14 @@ class RpmControl : public ModuleParams float getActuatorCorrection(); private: - uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] - float _rpm_estimate{0.f}; - float _spoolup_progress{0.f}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; PID _pid; - hrt_abstime _timestamp_last_rpm_measurement{0}; - hrt_abstime _timestamp_last_update{0}; + float _spoolup_progress{0.f}; // [0,1] + hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout + float _actuator_correction{0.f}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_heli_rpm_sp, From 6bd9c7015e71ec7bf095cae9d1cc043d104d3d73 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 19:28:20 +0100 Subject: [PATCH 09/11] Commander: start timer for auto disarm after spoolup --- src/modules/commander/Commander.cpp | 3 ++- src/modules/commander/Commander.hpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5fc8ee4d8f3c..fef03b92c28e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, + (_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e5bdb947aacb..81a95c721388 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -331,7 +331,6 @@ class Commander : public ModuleBase, public ModuleParams param_t _param_rc_map_fltmode{PARAM_INVALID}; DEFINE_PARAMETERS( - (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, @@ -348,6 +347,7 @@ class Commander : public ModuleBase, public ModuleParams (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, + (ParamFloat) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max From 214d710b40b761d179a1ba59b6b7095918cd0242 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 20:54:20 +0100 Subject: [PATCH 10/11] Helicopter defaults: don't auto disarm so quickly after spoolup --- ROMFS/px4fmu_common/init.d/rc.heli_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca2685e..93e6f0d33f01 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -12,6 +12,7 @@ param set-default MAV_TYPE 4 param set-default COM_PREARM_MODE 2 param set-default COM_SPOOLUP_TIME 10 +param set-default COM_DISARM_PRFLT 60 # No need for minimum collective pitch (or airmode) to keep torque authority param set-default MPC_MANTHR_MIN 0 From 0a016df0ffb83c76c50ddc4fd7c285582136fbae Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 21:02:51 +0100 Subject: [PATCH 11/11] RpmControl: Better consider the case where there's no rpm measurement (anymore) --- msg/Rpm.msg | 1 + .../control_allocator/ActuatorEffectiveness/RpmControl.cpp | 6 +++++- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index ca69e50fdb1c..b4de2cf42262 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +# rpm values of 0.0 mean within a timeout there is no movement measured float32 rpm_estimate # filtered revolutions per minute float32 rpm_raw diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index f61a392ad492..53d9766e2d0f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -68,11 +68,15 @@ float RpmControl::getActuatorCorrection() const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; } } // Timeout - if (now > _timestamp_last_measurement + 1_s) { + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { _pid.resetIntegral(); _actuator_correction = 0.f; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b412445048bd..5fd0c96d9115 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -63,6 +63,7 @@ class RpmControl : public ModuleParams static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; PID _pid; float _spoolup_progress{0.f}; // [0,1] hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout