Temporary RPM control for helicopters

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
-
-

ROMFS/px4fmu_common/init.d/rcS

@@ -388,6 +388,7 @@ else
 		commander start
 
 		dshot start
+		pwm_input start
 		pwm_out start
 	fi
 

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

msg/CMakeLists.txt

@@ -173,6 +173,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	Rpm.msg
+	RpmControlStatus.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg

msg/PwmInput.msg

@@ -1,4 +1,4 @@
-uint64 timestamp	# Time since system start (microseconds)
+uint64 timestamp   # Time since system start (microseconds)
-uint64 error_count	# Timer overcapture error flag (AUX5 or MAIN5)
+uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
-uint32 pulse_width	# Pulse width, timer counts
+uint32 pulse_width # Pulse width, timer counts (microseconds)
-uint32 period		# Period, timer counts
+uint32 period      # Period, timer counts (microseconds)

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

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) {

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 indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f;
-	float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;
+	float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1e6f) * 60.f;
 
 	// publish data to uorb
 	rpm_s msg{};

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

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);
+	}
+}

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 <cmath>
+
+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};
+};

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 <gtest/gtest.h>
+#include <PID.hpp>
+
+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);
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -135,8 +135,8 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	_saturation_flags = {};
 
 	// throttle/collective pitch curve
-	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
+	const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
-			       _geometry.throttle_curve) * throttleSpoolupProgress();
+				_geometry.throttle_curve) + _rpm_control.getActuatorCorrection()) * throttleSpoolupProgress();
 	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
 
 	// actuator mapping

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -41,6 +41,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/manual_control_switches.h>
 
+#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};
 };

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)

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 <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/pid/PID.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/rpm_control_status.h>
+
+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<float>(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_s> _rpm_control_status_pub{ORB_ID(rpm_control_status)};
+
+	float _rpm_raw{0.f};
+	AlphaFilter<float> _rpm_filter;
+	PID _pid;
+	hrt_abstime _timestamp_last_update{0};
+	hrt_abstime _timestamp_last_rpm_measurement{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CA_HELI_RPM_SP>) _param_ca_heli_rpm_sp,
+		(ParamFloat<px4::params::CA_HELI_RPM_P>) _param_ca_heli_rpm_p,
+		(ParamFloat<px4::params::CA_HELI_RPM_I>) _param_ca_heli_rpm_i
+	)
+};

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(&param_update);

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:

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);