Helicopter main rotor rpm control

ROMFS/px4fmu_common/init.d/rc.heli_defaults

@@ -12,6 +12,7 @@ param set-default MAV_TYPE 4
 
 param set-default COM_PREARM_MODE 2
 param set-default COM_SPOOLUP_TIME 10
+param set-default COM_DISARM_PRFLT 60
 
 # No need for minimum collective pitch (or airmode) to keep torque authority
 param set-default MPC_MANTHR_MIN 0

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)
 
+# rpm values of 0.0 mean within a timeout there is no movement measured
 float32 rpm_estimate # filtered revolutions per minute
 float32 rpm_raw

src/modules/commander/Commander.cpp

@@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm()
 				_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
 
 			} else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) {
-				_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s);
+				_auto_disarm_landed.set_hysteresis_time_from(false,
+						(_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s);
 				_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
 			}
 

src/modules/commander/Commander.hpp

@@ -331,7 +331,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	param_t _param_rc_map_fltmode{PARAM_INVALID};
 
 	DEFINE_PARAMETERS(
-
 		(ParamFloat<px4::params::COM_DISARM_LAND>)  _param_com_disarm_land,
 		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_prflt,
 		(ParamBool<px4::params::COM_DISARM_MAN>)    _param_com_disarm_man,
@@ -348,6 +347,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 		(ParamFloat<px4::params::COM_OBC_LOSS_T>)   _param_com_obc_loss_t,
 		(ParamInt<px4::params::COM_PREARM_MODE>)    _param_com_prearm_mode,
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_com_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_com_takeoff_act,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max

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 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe
 	bool _main_motor_engaged{true};
 
 	const ActuatorType _tail_actuator_type;
+
+	RpmControl _rpm_control{this};
 };

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -62,13 +62,16 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessTailsitterVTOL.hpp
 	ActuatorEffectivenessRoverAckermann.hpp
 	ActuatorEffectivenessRoverAckermann.cpp
+	RpmControl.cpp
+	RpmControl.hpp
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
+		PID
 )
 
 px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)

src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_pid.setOutputLimit(PID_OUTPUT_LIMIT);
+	_pid.setIntegralLimit(PID_OUTPUT_LIMIT);
+};
+
+void RpmControl::setSpoolupProgress(float spoolup_progress)
+{
+	_spoolup_progress = spoolup_progress;
+	_pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get());
+
+	if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) {
+		_pid.resetIntegral();
+	}
+}
+
+float RpmControl::getActuatorCorrection()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	// RPM measurement update
+	if (_rpm_sub.updated()) {
+		rpm_s rpm{};
+
+		if (_rpm_sub.copy(&rpm)) {
+			const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f);
+			_timestamp_last_measurement = rpm.timestamp;
+
+			const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.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);
+			_actuator_correction = _pid.update(rpm.rpm_estimate, dt, true);
+
+			_rpm_invalid = rpm.rpm_estimate < 1.f;
+		}
+	}
+
+	// Timeout
+	const bool timeout = now > _timestamp_last_measurement + 1_s;
+
+	if (_rpm_invalid || timeout) {
+		_pid.resetIntegral();
+		_actuator_correction = 0.f;
+	}
+
+	return _actuator_correction;
+}

src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 <[email protected]>
+ */
+
+#pragma once
+
+#include <lib/pid/PID.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rpm.h>
+
+class RpmControl : public ModuleParams
+{
+public:
+	RpmControl(ModuleParams *parent);
+	~RpmControl() = default;
+
+	void setSpoolupProgress(float spoolup_progress);
+	float getActuatorCorrection();
+
+private:
+	static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1]
+	static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1]
+
+	uORB::Subscription _rpm_sub{ORB_ID(rpm)};
+	bool _rpm_invalid{true};
+	PID _pid;
+	float _spoolup_progress{0.f}; // [0,1]
+	hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout
+	float _actuator_correction{0.f};
+
+	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: