diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7abae77293..d8d2393c00 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 fcc7dbe4ac..805d6fbd6b 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 0000000000..c1ed3dd997 --- /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 f403424b67..76481da71b 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 @@ private: 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 2406b81bf8..a203ac9821 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 0000000000..a5f413178e --- /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 48363eeb1e..0bc3f9c86e 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 d085704df4..013b954033 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: