mc rate control: add gyroscopic torque compensation loop

Single (rigid) rotor aircrafts have their dynamics dominated by the
gyroscopic torque. Compensating for this effect by a feedback greatly
improves stability.
Multirotors can also benefit from this feature when the difference in
RPM between the CW and CCW rotors is large (the sum of the angular
momentum of the rotors is non zero).

src/lib/rate_control/rate_control.cpp

@@ -74,8 +74,12 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 	// angular rates error
 	Vector3f rate_error = rate_sp - rate;
 
+	// assume the rotor angular momentum is aligned with the Z axis
+	const Vector3f gyroscopic_torque = rate.cross(Vector3f(0.f, 0.f, _rotor_angular_momentum));
+
 	// PID control with feed forward
-	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
+	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(
+					rate_sp) - gyroscopic_torque;
 
 	// update integral only if we are not landed
 	if (!landed) {

src/lib/rate_control/rate_control.hpp

@@ -71,6 +71,12 @@ public:
 	 */
 	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
 
+	/**
+	 * Set the total angular momentum produced by the rotors
+	 * @param norm of angular momentum along the Z body axis
+	 */
+	void setRotorAngularMomentum(float momentum) { _rotor_angular_momentum = momentum; };
+
 	/**
 	 * Set saturation status
 	 * @param control saturation vector from control allocator
@@ -129,6 +135,7 @@ private:
 	matrix::Vector3f _gain_d; ///< rate control derivative gain
 	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
 	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
+	float _rotor_angular_momentum{0.f}; ///< used to cancel out the gyroscopic torque generated by the rotors
 
 	// States
 	matrix::Vector3f _rate_int; ///< integral term of the rate controller

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -186,3 +186,11 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control,
 		actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler;
 	}
 }
+
+void ActuatorEffectivenessControlSurfaces::applyScale(float scale, int first_actuator_idx,
+		ActuatorVector &actuator_sp) const
+{
+	for (int i = 0; i < _count; ++i) {
+		actuator_sp(i + first_actuator_idx) *= scale;
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -91,6 +91,7 @@ public:
 
 	void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
 	void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
+	void applyScale(float scale, int first_actuator_idx, ActuatorVector &actuator_sp) const;
 
 private:
 	void updateParams() override;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "ActuatorEffectivenessThrustVectoring.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessThrustVectoring::ActuatorEffectivenessThrustVectoring(ModuleParams *parent)
+	: ModuleParams(parent),
+	  _rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::FixedUpwards),
+	  _control_surfaces(this)
+{
+}
+
+bool
+ActuatorEffectivenessThrustVectoring::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	// Motors
+	_rotors.enablePropellerTorque(false);
+	const bool rotors_added_successfully = _rotors.addActuators(configuration);
+
+	// Control Surfaces
+	_first_control_surface_idx = configuration.num_actuators_matrix[0];
+	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
+
+	return (rotors_added_successfully && surfaces_added_successfully);
+}
+
+void ActuatorEffectivenessThrustVectoring::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	esc_status_s esc_status;
+	hrt_abstime now = hrt_absolute_time();
+	float rpm = 0.f;
+
+	if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + 1_s)) {
+
+		for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) {
+			const esc_report_s &esc_report = esc_status.esc[esc];
+
+			const bool esc_connected = (esc_status.esc_online_flags & (1 << esc)) || (esc_report.esc_rpm != 0);
+
+			if (esc_connected && (now < esc_report.timestamp + 1_s)) {
+				rpm = esc_report.esc_rpm;
+				break;
+			}
+		}
+	}
+
+	constexpr float propeller_coefficient = 1.f / (20000.f * 20000.f); // scale down the thrust
+	float thrust = rpm * rpm * propeller_coefficient;
+
+	// Use thrust setpoint if there is no ESC telemetry available
+	if (thrust < FLT_EPSILON) {
+		vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
+
+		if (_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint)) {
+			thrust = vehicle_thrust_setpoint.xyz[2];
+		}
+	}
+
+	const float scale = 1.f / fmaxf(thrust, 0.2f);
+	_control_surfaces.applyScale(scale, _first_control_surface_idx, actuator_sp);
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.hpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+#include "ActuatorEffectivenessControlSurfaces.hpp"
+
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+
+class ActuatorEffectivenessThrustVectoring : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessThrustVectoring(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessThrustVectoring() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	const char *name() const override { return "Thrust Vectoring"; }
+
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+private:
+	static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
+
+	ActuatorEffectivenessRotors _rotors;
+	ActuatorEffectivenessControlSurfaces _control_surfaces;
+
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::Subscription _esc_status_sub {ORB_ID(esc_status)};
+
+	int _first_control_surface_idx{0};
+};

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -50,6 +50,8 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessMCTilt.hpp
 	ActuatorEffectivenessMultirotor.cpp
 	ActuatorEffectivenessMultirotor.hpp
+	ActuatorEffectivenessThrustVectoring.cpp
+	ActuatorEffectivenessThrustVectoring.hpp
 	ActuatorEffectivenessTilts.cpp
 	ActuatorEffectivenessTilts.hpp
 	ActuatorEffectivenessRotors.cpp

src/modules/control_allocator/ControlAllocator.cpp

@@ -270,6 +270,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessHelicopterCoaxial(this);
 			break;
 
+		case EffectivenessSource::THRUST_VECTORING:
+			tmp = new ActuatorEffectivenessThrustVectoring(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;

src/modules/control_allocator/ControlAllocator.hpp

@@ -44,6 +44,7 @@
 #include <ActuatorEffectiveness.hpp>
 #include <ActuatorEffectivenessMultirotor.hpp>
 #include <ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectivenessThrustVectoring.hpp>
 #include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 #include <ActuatorEffectivenessTailsitterVTOL.hpp>
 #include <ActuatorEffectivenessRoverAckermann.hpp>
@@ -158,6 +159,7 @@ private:
 		HELICOPTER_TAIL_ESC = 10,
 		HELICOPTER_TAIL_SERVO = 11,
 		HELICOPTER_COAXIAL = 12,
+		THRUST_VECTORING = 13,
 	};
 
 	enum class FailureMode {

src/modules/control_allocator/module.yaml

@@ -30,6 +30,7 @@ parameters:
                 10: Helicopter (tail ESC)
                 11: Helicopter (tail Servo)
                 12: Helicopter (Coaxial)
+                13: Thrust Vectoring
             default: 0
 
         CA_METHOD:

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -183,6 +183,28 @@ MulticopterRateControl::Run()
 			}
 		}
 
+		if (_esc_status_sub.updated()) {
+			esc_status_s esc_status;
+
+			if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + 1_s)) {
+				float rpm_sum = 0.f;
+
+				for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) {
+					const esc_report_s &esc_report = esc_status.esc[esc];
+
+					const bool esc_connected = (esc_status.esc_online_flags & (1 << esc)) || (esc_report.esc_rpm != 0);
+
+					// assuming all the rotors are in the same plane, the gyroscopic torque is proportional to the
+					// difference between the CW and CCW rotation speed
+					if (esc_connected && (now < esc_report.timestamp + 1_s)) {
+						rpm_sum += esc_report.esc_rpm;
+					}
+				}
+
+				_rate_control.setRotorAngularMomentum(rpm_sum * _param_mc_precess_gain.get());
+			}
+		}
+
 		// run the rate controller
 		if (_vehicle_control_mode.flag_control_rates_enabled) {
 

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -49,6 +49,7 @@
 #include <uORB/topics/actuator_controls_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
@@ -93,6 +94,7 @@ private:
 
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
+	uORB::Subscription _esc_status_sub {ORB_ID(esc_status)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
@@ -109,6 +111,8 @@ private:
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub;
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub;
 
+	static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
+
 	vehicle_control_mode_s	_vehicle_control_mode{};
 	vehicle_status_s	_vehicle_status{};
 
@@ -151,6 +155,8 @@ private:
 		(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
 		(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
 
+		(ParamFloat<px4::params::MC_PRECESS_GAIN>) _param_mc_precess_gain,
+
 		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
 		(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
 		(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,

src/modules/mc_rate_control/mc_rate_control_params.c

@@ -279,6 +279,22 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
 
+/**
+ * Gyroscopic torque compensation gain
+ *
+ * Compensates for the gyroscopic torque generated
+ * by a difference in speed between the CW and CCW rotors.
+ *
+ * Assumes that all the rotors have the same inertia and
+ * their thrust is pointing upwards.
+ *
+ * @min -1.0
+ * @max  1.0
+ * @decimal 8
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PRECESS_GAIN, 0.0f);
+
 /**
  * Battery power level scaler
  *