From 1c4325db6d299eb11454bcbbfdfa043ff4d188b8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:30:43 +0100 Subject: [PATCH] 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 a203ac9821..10ff984b29 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 0000000000..56bccfe779 --- /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 396183382f..b5930902db 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)};