RpmControl: class clean up

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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

ROMFS/px4fmu_common/init.d/rcS

@@ -354,6 +354,11 @@ else
 	then
 		pps_capture start
 	fi
+	# RPM capture driver
+	if param greater -s RPM_CAP_ENABLE 0
+	then
+		rpm_capture start
+	fi
 	# Camera capture driver
 	if param greater -s CAM_CAP_FBACK 0
 	then

msg/CMakeLists.txt

@@ -193,6 +193,7 @@ set(msg_files
 	RoverMecanumSetpoint.msg
 	RoverMecanumStatus.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 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)

msg/Rpm.msg

@@ -1,4 +1,5 @@
 uint64 timestamp                      # time since system start (microseconds)
 
-float32 indicated_frequency_rpm       # indicated rotor Frequency in Revolution per minute
-float32 estimated_accurancy_rpm       # estimated accuracy in Revolution per minute
+float32 rpm_estimate # filtered revolutions per minute
+float32 rpm_raw # measured rpm
+float32 estimated_accurancy_rpm # estimated accuracy

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; // 1us = 1mm 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,12 +195,12 @@ 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{};
-	msg.indicated_frequency_rpm = indicated_rpm;
+	msg.rpm_estimate = indicated_rpm;
 	msg.estimated_accurancy_rpm = estimated_accurancy;
 	msg.timestamp = hrt_absolute_time();
 	_rpm_pub.publish(msg);

src/drivers/rpm/rpm_simulator/rpm_simulator.cpp

@@ -66,7 +66,7 @@ int rpm_simulator_main(int argc, char *argv[])
 
 	// prpepare RPM data message
 	rpm.timestamp = timestamp_us;
-	rpm.indicated_frequency_rpm = frequency;
+	rpm.rpm_estimate = frequency;
 	rpm.estimated_accurancy_rpm = frequency / 100.0f;
 
 	// Publish data and let the user know what was published

src/drivers/rpm_capture/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2021 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.
+#
+############################################################################
+
+set(PARAM_PREFIX PWM_MAIN)
+
+if(CONFIG_BOARD_IO)
+	set(PARAM_PREFIX PWM_AUX)
+endif()
+
+px4_add_module(
+	MODULE drivers__rpm_capture
+	MAIN rpm_capture
+	COMPILE_FLAGS
+		-DPARAM_PREFIX="${PARAM_PREFIX}"
+	SRCS
+		RPMCapture.cpp
+	)

src/drivers/rpm_capture/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RPM_CAPTURE
+	bool "rpm_capture"
+	default n
+	---help---
+		Enable support for rpm_capture

src/drivers/rpm_capture/RPMCapture.cpp

@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "RPMCapture.hpp"
+#include <px4_arch/io_timer.h>
+#include <board_config.h>
+#include <parameters/param.h>
+#include <px4_platform_common/events.h>
+#include <systemlib/mavlink_log.h>
+
+RPMCapture::RPMCapture() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	ModuleParams(nullptr)
+{
+	_pwm_input_pub.advertise();
+	ScheduleNow();
+}
+
+RPMCapture::~RPMCapture()
+{
+	if (_channel >= 0) {
+		io_timer_unallocate_channel(_channel);
+		px4_arch_gpiosetevent(_rpm_capture_gpio, false, false, false, nullptr, nullptr);
+	}
+}
+
+bool RPMCapture::init()
+{
+	bool success = false;
+
+	for (unsigned i = 0; i < 16; ++i) {
+		char param_name[17];
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+		param_t function_handle = param_find(param_name);
+		int32_t function;
+
+		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+			if (function == 2070) { // RPM_Input
+				_channel = i;
+			}
+		}
+	}
+
+	if (_channel == -1) {
+		PX4_WARN("No RPM channel configured");
+		return false;
+	}
+
+	int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_Other); // TODO: add IOTimerChanMode_RPM
+
+	if (ret != PX4_OK) {
+		PX4_ERR("gpio alloc failed (%i) for RPM at channel (%d)", ret, _channel);
+		return false;
+	}
+
+	_rpm_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel));
+	int ret_val = px4_arch_gpiosetevent(_rpm_capture_gpio, true, false, true, &RPMCapture::gpio_interrupt_callback, this);
+
+	if (ret_val == PX4_OK) {
+		success = true;
+	}
+
+	success = success && _rpm_pub.advertise();
+	return success;
+}
+
+void RPMCapture::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_interrupt_happened.load()) {
+		// There was an interrupt
+		_period = _hrt_timestamp - _hrt_timestamp_prev;
+		_hrt_timestamp_prev = _hrt_timestamp;
+		_interrupt_happened.store(false);
+
+		pwm_input_s pwm_input{};
+		pwm_input.timestamp = now;
+		pwm_input.period = _period;
+		pwm_input.error_count = _error_count;
+		_pwm_input_pub.publish(pwm_input);
+
+		ScheduleClear(); // Do not run on previously scheduled timeout
+
+	} else {
+		// Timeout for no interrupts
+		_period = UINT32_MAX;
+	}
+
+	ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout
+	float rpm_raw{0.f};
+
+	if (_period < RPM_PULSE_TIMEOUT) {
+		// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
+		rpm_raw = 60.f * 1e6f / static_cast<float>(_param_rpm_puls_per_rev.get() * _period);
+
+	} else {
+		_rpm_filter.reset(rpm_raw);
+	}
+
+	if (rpm_raw < RPM_MAX_VALUE) {
+		// Don't update RPM filter with outliers
+		const float dt = math::min((now - _timestamp_last_update) * 1e-6f, 1.f);
+		_timestamp_last_update = now;
+		_rpm_filter.setParameters(dt, 0.5f);
+		_rpm_filter.update(_rpm_median_filter.apply(rpm_raw));
+	}
+
+	rpm_s rpm{};
+	rpm.timestamp = now;
+	rpm.rpm_raw = rpm_raw;
+	rpm.rpm_estimate = _rpm_filter.getState();
+	_rpm_pub.publish(rpm);
+}
+
+int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
+{
+	RPMCapture *instance = static_cast<RPMCapture *>(arg);
+
+	if (instance->_interrupt_happened.load()) {
+		++instance->_error_count;
+	}
+
+	instance->_hrt_timestamp = hrt_absolute_time();
+	instance->_interrupt_happened.store(true);
+	instance->ScheduleNow();
+
+	return PX4_OK;
+}
+
+int RPMCapture::task_spawn(int argc, char *argv[])
+{
+	RPMCapture *instance = new RPMCapture();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int RPMCapture::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int RPMCapture::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_USAGE_NAME("rpm_capture", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+void RPMCapture::stop()
+{
+	exit_and_cleanup();
+}
+
+extern "C" __EXPORT int rpm_capture_main(int argc, char *argv[])
+{
+	if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running()) {
+		RPMCapture::stop();
+	}
+
+	return RPMCapture::main(argc, argv);
+}

src/drivers/rpm_capture/RPMCapture.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
+#include <px4_arch/micro_hal.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/rpm.h>
+
+using namespace time_literals;
+
+class RPMCapture : public ModuleBase<RPMCapture>, public px4::ScheduledWorkItem, public ModuleParams
+{
+public:
+	RPMCapture();
+	virtual ~RPMCapture();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	static int gpio_interrupt_callback(int irq, void *context, void *arg);
+
+	/** RPMCapture is an interrupt-driven task and needs to be manually stopped */
+	static void stop();
+
+private:
+	static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s;
+	static constexpr float RPM_MAX_VALUE = 50e3f;
+
+	void Run() override;
+
+	int _channel{-1};
+	uint32_t _rpm_capture_gpio{0};
+	uORB::Publication<pwm_input_s> _pwm_input_pub{ORB_ID(pwm_input)};
+	uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};
+
+	hrt_abstime _hrt_timestamp{0};
+	hrt_abstime _hrt_timestamp_prev{0};
+	uint32_t _period{UINT32_MAX};
+	uint32_t _error_count{0};
+	px4::atomic<bool> _interrupt_happened{false};
+
+	hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt
+	AlphaFilter<float> _rpm_filter;
+	MedianFilter<float, 5> _rpm_median_filter;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::RPM_PULS_PER_REV>) _param_rpm_puls_per_rev
+	)
+};

src/drivers/rpm_capture/rpm_capture_params.c

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * RPM Capture Enable
+ *
+ * Enables the RPM capture module on FMU channel 5.
+ *
+ * @boolean
+ * @group System
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0);
+
+/**
+ * Voltage pulses per revolution
+ *
+ * Number of voltage pulses per one rotor revolution on the capturing pin.
+ *
+ * @group System
+ * @min 1
+ * @max 50
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(RPM_PULS_PER_REV, 1);

src/lib/mixer_module/output_functions.yaml

@@ -60,3 +60,9 @@ functions:
         condition: "PPS_CAP_ENABLE==0"
         text: "PPS needs to be enabled via PPS_CAP_ENABLE parameter."
       exclude_from_actuator_testing: true
+    RPM_Input:
+      start: 2070
+      note:
+        condition: "RPM_CAP_ENABLE==0"
+        text: "RPM needs to be enabled via RPM_CAP_ENABLE parameter."
+      exclude_from_actuator_testing: true

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 {
 	_saturation_flags = {};
 
+	const float spoolup_progress = throttleSpoolupProgress();
+	_rpm_control.setSpoolupProgress(spoolup_progress);
+
 	// throttle/collective pitch curve
-	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
-			       _geometry.throttle_curve) * throttleSpoolupProgress();
+	const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
+				_geometry.throttle_curve) + _rpm_control.getActuatorCorrection()) * spoolup_progress;
 	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,132 @@
+/****************************************************************************
+ *
+ *   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.h>
+#include <uORB/topics/rpm_control_status.h>
+
+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 (_rpm_sub.updated()) {
+			rpm_s rpm_input{};
+			if (_rpm_sub.copy(&rpm_input)) {
+				_rpm_estimate = rpm_input.rpm_estimate;
+				_rpm_raw = rpm_input.rpm_raw;
+				_timestamp_last_rpm_measurement = rpm_input.timestamp;
+			}
+		}
+
+		hrt_abstime current_time = hrt_absolute_time();
+		const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f);
+		_timestamp_last_update = current_time;
+
+		const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s;
+		const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE;
+
+		if (rpm_measurement_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_estimate, dt, true);
+
+		rpm_control_status_s rpm_control_status{};
+		rpm_control_status.rpm_raw = _rpm_raw;
+		rpm_control_status.rpm_estimate = _rpm_estimate;;
+		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:
+	static constexpr float RPM_MAX_VALUE = 1800.f;
+	uORB::Subscription _rpm_sub{ORB_ID(rpm)};
+	uORB::Publication<rpm_control_status_s> _rpm_control_status_pub{ORB_ID(rpm_control_status)};
+
+	float _rpm_estimate{0.f};
+	float _rpm_raw{0.f};
+	float _spoolup_progress{0.f};
+	PID _pid;
+	hrt_abstime _timestamp_last_rpm_measurement{0};
+	hrt_abstime _timestamp_last_update{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

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

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);
@@ -101,6 +102,7 @@ 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("rover_ackermann_guidance_status", 100);
@@ -112,6 +114,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("rover_mecanum_guidance_status", 100);
 	add_optional_topic("rover_mecanum_setpoint", 100);
 	add_optional_topic("rover_mecanum_status", 100);
+	add_optional_topic_multi("rpm_control_status");
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

src/modules/mavlink/mavlink_main.cpp

@@ -1777,6 +1777,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("PING", 0.1f);
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f);
 		configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
+		configure_stream_local("RAW_RPM", 5.0f);
 		configure_stream_local("RC_CHANNELS", 20.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream_local("SYS_STATUS", 5.0f);

src/modules/mavlink/streams/RAW_RPM.hpp

@@ -68,7 +68,7 @@ private:
 				mavlink_raw_rpm_t msg{};
 
 				msg.index = i;
-				msg.frequency = rpm.indicated_frequency_rpm;
+				msg.frequency = rpm.rpm_estimate;
 
 				mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg);
 				updated = true;

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -388,14 +388,12 @@ void SimulatorMavlink::handle_message(const mavlink_message_t *msg)
 		break;
 
 	case MAVLINK_MSG_ID_RAW_RPM:
-		mavlink_raw_rpm_t rpm;
-		mavlink_msg_raw_rpm_decode(msg, &rpm);
-		rpm_s rpmmsg{};
-		rpmmsg.timestamp = hrt_absolute_time();
-		rpmmsg.indicated_frequency_rpm = rpm.frequency;
-		rpmmsg.estimated_accurancy_rpm = 0;
-
-		_rpm_pub.publish(rpmmsg);
+		mavlink_raw_rpm_t rpm_mavlink;
+		mavlink_msg_raw_rpm_decode(msg, &rpm_mavlink);
+		rpm_s rpm_uorb{};
+		rpm_uorb.timestamp = hrt_absolute_time();
+		rpm_uorb.rpm_estimate = rpm_mavlink.frequency;
+		_rpm_pub.publish(rpm_uorb);
 		break;
 	}
 }