TVC: fix unallocated torque calculation

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/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -200,6 +200,8 @@ public:
 				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
 
+	const ActuatorVector &getActuatorEffectivenessScale() { return _actuator_effectiveness_scale; }
+
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
@@ -222,4 +224,6 @@ public:
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	uint32_t _stopped_motors_mask{0};
+
+	ActuatorVector _actuator_effectiveness_scale;
 };

src/lib/control_allocation/control_allocation/ControlAllocation.hpp

@@ -143,7 +143,7 @@ public:
 	 * @return Control vector
 	 */
 	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
-	{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
+	{ return (_effectiveness * (_actuator_sp - _actuator_trim).emult(_actuator_effectiveness_scale)).emult(_control_allocation_scale); }
 
 	/**
 	 * Get the control effectiveness matrix
@@ -233,12 +233,15 @@ protected:
 
 	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  ///< Effectiveness matrix
 	matrix::Vector<float, NUM_AXES> _control_allocation_scale;  	///< Scaling applied during allocation
+	matrix::Vector<float, NUM_AXES> _actuator_sp_scale;  	///< Scaling durectly applied to the actuator setpoint
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; 	///< Neutral actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_min; 	///< Minimum actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_max; 	///< Maximum actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_slew_rate_limit; 	///< Slew rate limit
 	matrix::Vector<float, NUM_ACTUATORS> _prev_actuator_sp;  	///< Previous actuator setpoint
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_sp;  	///< Actuator setpoint
+	matrix::Vector<float, NUM_ACTUATORS> _actuator_effectiveness_scale; ///< Scaling applied after inilial
+	///allocation (e.g.: when effectiveness changes due to airspeed or thrust)
 	matrix::Vector<float, NUM_AXES> _control_sp;   		///< Control setpoint
 	matrix::Vector<float, NUM_AXES> _control_trim; 		///< Control at trim actuator values
 	int _num_actuators{0};

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/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;
@@ -438,6 +442,7 @@ ControlAllocator::Run()
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
 								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
+			_control_allocation[i]->_actuator_effectiveness_scale = _actuator_effectiveness->getActuatorEffectivenessScale();
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);

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 = 14,
 	};
 
 	enum class FailureMode {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -186,3 +186,18 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control,
 		actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler;
 	}
 }
+
+void ActuatorEffectivenessControlSurfaces::applyEffectivenessScale(float effectiveness_scale, int first_actuator_idx,
+		ActuatorVector &actuator_sp, ActuatorVector &applied_effectiveness_scale) const
+{
+	if (!PX4_ISFINITE(effectiveness_scale)) {
+		effectiveness_scale = 1.f;
+	}
+
+	const float actuator_scale = 1.f / fmaxf(effectiveness_scale, FLT_EPSILON);
+
+	for (int i = 0; i < _count; ++i) {
+		actuator_sp(i + first_actuator_idx) *= actuator_scale;
+		applied_effectiveness_scale(i + first_actuator_idx) = effectiveness_scale;
+	}
+}

src/modules/control_allocator/VehicleActuatorEffectiveness/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 applyEffectivenessScale(float scale, int first_actuator_idx, ActuatorVector &actuator_sp, ActuatorVector &applied_effectiveness_scale) const;
 
 private:
 	void updateParams() override;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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;
+			}
+		}
+	}
+
+	const  float rpm_scaled = rpm / 20000.f; // scale down the thrust to keep values in a reasonable range
+	float thrust = rpm_scaled * rpm_scaled;
+
+	// 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 effectiveness_scale = fmaxf(thrust, 0.2f);
+	_control_surfaces.applyEffectivenessScale(effectiveness_scale, _first_control_surface_idx, actuator_sp,
+			_actuator_effectiveness_scale);
+}

src/modules/control_allocator/VehicleActuatorEffectiveness/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/VehicleActuatorEffectiveness/CMakeLists.txt

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

src/modules/control_allocator/module.yaml

@@ -31,6 +31,7 @@ parameters:
                 11: Helicopter (tail Servo)
                 12: Helicopter (Coaxial)
                 13: Rover (Mecanum)
+                14: Thrust Vectoring
             default: 0
 
         CA_METHOD:

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -185,6 +185,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

@@ -50,6 +50,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>
@@ -94,6 +95,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)};
@@ -110,6 +112,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{};
 
@@ -155,6 +159,8 @@ private:
 		(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
 		(ParamFloat<px4::params::MC_YAW_TQ_CUTOFF>) _param_mc_yaw_tq_cutoff,
 
+		(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
  *