From 2ff9c558f9bee09c724f5b7e815df42e422b80eb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 1 Mar 2024 20:52:00 +0100 Subject: [PATCH] Temporary RPM control for helicopters --- .../init.d/airframes/16001_helicopter | 3 - ROMFS/px4fmu_common/init.d/rcS | 1 + boards/px4/fmu-v5x/default.px4board | 3 +- msg/CMakeLists.txt | 1 + msg/PwmInput.msg | 8 +- msg/RpmControlStatus.msg | 7 + .../ll40ls_pwm/LidarLitePWM.cpp | 2 +- src/drivers/rpm/pcf8583/PCF8583.cpp | 4 +- src/lib/pid/CMakeLists.txt | 12 +- src/lib/pid/PID.cpp | 65 +++++++++ src/lib/pid/PID.hpp | 61 +++++++++ src/lib/pid/PIDTest.cpp | 129 ++++++++++++++++++ .../ActuatorEffectivenessHelicopter.cpp | 4 +- .../ActuatorEffectivenessHelicopter.hpp | 4 + .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../ActuatorEffectiveness/RpmControl.hpp | 118 ++++++++++++++++ .../control_allocator/ControlAllocator.cpp | 2 +- src/modules/control_allocator/module.yaml | 35 +++++ src/modules/logger/logged_topics.cpp | 3 + 19 files changed, 448 insertions(+), 16 deletions(-) create mode 100644 msg/RpmControlStatus.msg create mode 100644 src/lib/pid/PID.cpp create mode 100644 src/lib/pid/PID.hpp create mode 100644 src/lib/pid/PIDTest.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp diff --git a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter index 36f1a68f14..695604e0b1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter +++ b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter @@ -11,7 +11,6 @@ . ${R}etc/init.d/rc.heli_defaults - # Disable PID gains for initial setup. These should be enabled after setting the FF gain. # P is expected to be lower than FF. param set-default MC_ROLLRATE_P 0 @@ -24,5 +23,3 @@ param set-default MC_PITCHRATE_D 0 param set-default MC_PITCHRATE_FF 0.1 param set-default CA_AIRFRAME 10 - - diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 466fed1f62..5318565e5a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -388,6 +388,7 @@ else commander start dshot start + pwm_input start pwm_out start fi diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index e2b5833dfe..ffd9f53749 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -38,6 +38,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y +CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y @@ -81,7 +82,7 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 62544cffc0..2eaec99eec 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -173,6 +173,7 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.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/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index bba9ff638c..e1333dffda 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp @@ -109,7 +109,7 @@ LidarLitePWM::measure() return PX4_ERROR; } - const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ + const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 1 usec = 1 mm distance for LIDAR-Lite */ /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ if (current_distance <= 0.0f) { diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index fcc9828dad..085015247f 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/src/drivers/rpm/pcf8583/PCF8583.cpp @@ -195,8 +195,8 @@ void PCF8583::RunImpl() } // Calculate RPM and accuracy estimation - float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f; - float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f; + float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f; + float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1e6f) * 60.f; // publish data to uorb rpm_s msg{}; diff --git a/src/lib/pid/CMakeLists.txt b/src/lib/pid/CMakeLists.txt index b8ed30b7a2..3c1e349c8b 100644 --- a/src/lib/pid/CMakeLists.txt +++ b/src/lib/pid/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2022 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 @@ -31,4 +31,12 @@ # ############################################################################ -px4_add_library(pid pid.cpp) +px4_add_library(PID + PID.cpp + PID.hpp +) +target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID) + +px4_add_library(pid pid.cpp) # TODO: Remove duplication diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp new file mode 100644 index 0000000000..3f0d9b4b93 --- /dev/null +++ b/src/lib/pid/PID.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2022-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 "PID.hpp" +#include "lib/mathlib/math/Functions.hpp" + +void PID::setGains(const float P, const float I, const float D) +{ + _gain_proportional = P; + _gain_integral = I; + _gain_derivative = D; +} + +float PID::update(const float feedback, const float dt, const bool update_integral) +{ + const float error = _setpoint - feedback; + const float feedback_change = std::isfinite(_last_feedback) ? (feedback - _last_feedback) / dt : 0.f; + const float output = (_gain_proportional * error) + _integral + (_gain_derivative * feedback_change); + + if (update_integral) { + updateIntegral(error, dt); + } + + _last_feedback = feedback; + return math::constrain(output, -_limit_output, _limit_output); +} + +void PID::updateIntegral(float error, const float dt) +{ + const float integral_new = _integral + _gain_integral * error * dt; + + if (std::isfinite(integral_new)) { + _integral = math::constrain(integral_new, -_limit_integral, _limit_integral); + } +} diff --git a/src/lib/pid/PID.hpp b/src/lib/pid/PID.hpp new file mode 100644 index 0000000000..81d573e72d --- /dev/null +++ b/src/lib/pid/PID.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#pragma once + +#include + +class PID +{ +public: + void setOutputLimit(const float limit) { _limit_output = limit; } + void setIntegralLimit(const float limit) { _limit_integral = limit; } + void setGains(const float P, const float I, const float D); + void setSetpoint(const float setpoint) { _setpoint = setpoint; } + float update(const float feedback, const float dt, const bool update_integral); + void resetIntegral() { _integral = 0.f; }; + void resetDerivative() { _last_feedback = NAN; }; +private: + void updateIntegral(float error, const float dt); + + float _setpoint{0.f}; ///< current setpoint to track + float _integral{0.f}; ///< integral state + float _last_feedback{NAN}; + + // Gains, Limits + float _gain_proportional{0.f}; + float _gain_integral{0.f}; + float _gain_derivative{0.f}; + float _limit_integral{0.f}; + float _limit_output{0.f}; +}; diff --git a/src/lib/pid/PIDTest.cpp b/src/lib/pid/PIDTest.cpp new file mode 100644 index 0000000000..ed3c204e6e --- /dev/null +++ b/src/lib/pid/PIDTest.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2022 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 +#include + +TEST(PIDTest, AllZeroCase) +{ + PID pid; + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f); +} + +TEST(PIDTest, OutputLimit) +{ + PID pid; + pid.setOutputLimit(.01f); + pid.setGains(.1f, 0.f, 0.f); + pid.setSetpoint(1.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f); + EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f); + EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f); + EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f); + EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f); + EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f); +} + +TEST(PIDTest, ProportinalOnly) +{ + PID pid; + pid.setOutputLimit(1.f); + pid.setGains(.1f, 0.f, 0.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f); + pid.setSetpoint(1.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f); + + float plant = 0.f; + float output = 10000.f; + int i; // need function scope to check how many steps + + for (i = 1000; i > 0; i--) { + const float output_new = pid.update(plant, 0.f, false); + plant += output_new; + + // expect the output to get smaller with each iteration + if (output_new >= output) { + break; + } + + output = output_new; + } + + EXPECT_FLOAT_EQ(plant, 1.f); + EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge +} + +TEST(PIDTest, InteralOpenLoop) +{ + PID pid; + pid.setOutputLimit(1.f); + pid.setGains(0.f, .1f, 0.f); + pid.setIntegralLimit(.05f); + pid.setSetpoint(1.f); + + // Zero error + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f); + + // Open loop ramp up + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + + // Open loop ramp down + pid.setSetpoint(-1.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); + pid.resetIntegral(); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b67..3a38c87c31 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -135,8 +135,8 @@ 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..df1f54c650 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + ~RpmControl() = default; + + float getActuatorCorrection() + { + if (_pwm_input_sub.updated()) { + pwm_input_s pwm_input{}; + + if (_pwm_input_sub.copy(&pwm_input)) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + if ((1 < pwm_input.period) && (pwm_input.period < 10000)) { + _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 18.f); + + } else { + _rpm_raw = 0; + } + + const float dt = math::constrain((pwm_input.timestamp - _timestamp_last_rpm_measurement) * 1e-6f, 0.01f, 1.f); + _timestamp_last_rpm_measurement = pwm_input.timestamp; + _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.update(_rpm_raw); + } + } + + 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; + + _pid.setGains(_param_ca_heli_rpm_p.get() * 1e-3f, _param_ca_heli_rpm_i.get() * 1e-3f, 0.f); + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + _pid.setSetpoint(_param_ca_heli_rpm_sp.get()); + float output = _pid.update(_rpm_filter.getState(), dt, true); + //float output = _param_ca_heli_rpm_p.get() * (_rpm_setpoint - _rpm_filter.getState()) * 1e-3f; + + 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); + + return output; + } + +private: + uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; + + float _rpm_raw{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 2ead22c570..6e97eed73f 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 8683d7477d..68856d63ef 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -527,6 +527,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: diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 68b3d27b42..34d6d37868 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -48,6 +48,7 @@ void LoggedTopics::add_default_topics() add_topic("action_request"); add_topic("actuator_armed"); add_optional_topic("actuator_controls_status_0", 300); + add_topic("actuator_test"); add_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); add_optional_topic("autotune_attitude_control_status", 100); @@ -99,8 +100,10 @@ void LoggedTopics::add_default_topics() add_topic("position_controller_landing_status", 100); add_topic("goto_setpoint", 200); add_topic("position_setpoint_triplet", 200); + add_topic("pwm_input"); add_optional_topic("px4io_status"); add_topic("radio_status"); + add_optional_topic_multi("rpm_control_status"); add_topic("rtl_time_estimate", 1000); add_optional_topic("rtl_status", 5000); add_optional_topic("sensor_airflow", 100);