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 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/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/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 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..10ff984b2917 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.cpp + RpmControl.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -69,6 +71,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.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 000000000000..53d9766e2d0f --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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) +{ + _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 < 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)) { + 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); + + _rpm_invalid = rpm.rpm_estimate < 1.f; + } + } + + // Timeout + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { + _pid.resetIntegral(); + _actuator_correction = 0.f; + } + + return _actuator_correction; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp new file mode 100644 index 000000000000..5fd0c96d9115 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent); + ~RpmControl() = default; + + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); + +private: + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + 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 + float _actuator_correction{0.f}; + + 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: