ControlAllocator: introduce helicopter rotor rpm controller

msg/CMakeLists.txt

@@ -181,6 +181,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/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/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,150 @@
+/****************************************************************************
+ *
+ *   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 (_pwm_input_sub.updated()) {
+			pwm_input_s pwm_input{};
+
+			if (_pwm_input_sub.copy(&pwm_input)) {
+				if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) {
+					// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
+					_rpm_raw = 60.f * 1e6f / (static_cast<float>(pwm_input.period) * 1.f);
+
+				} else {
+					_rpm_raw = 0;
+				}
+
+				_timestamp_last_rpm_measurement = pwm_input.timestamp;
+			}
+		}
+
+		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;
+
+		_rpm_filter.setParameters(dt, 0.5f);
+		_rpm_filter.update(_rpm_raw);
+
+		const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s);
+		const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f;
+
+		if (no_rpm_pulse_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_filter.getState(), dt, true);
+
+		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);
+
+		// Publish estimated rpm for MAVLink -> UI in ground station
+		rpm_s rpm{
+			.timestamp = hrt_absolute_time(),
+			.indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm
+		};
+
+		_rpm_pub.publish(rpm);
+
+		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)};
+	uORB::Publication<rpm_s> _rpm_pub {ORB_ID::rpm};
+
+	float _rpm_raw{0.f};
+	float _spoolup_progress{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

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