TVC: fix unallocated torque calculation

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -200,6 +200,8 @@ class ActuatorEffectiveness
 				    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 @@ class ActuatorEffectiveness
 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 @@ class ControlAllocation
 	 * @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 @@ class ControlAllocation
 
 	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/modules/control_allocator/ControlAllocator.cpp

@@ -442,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/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -187,10 +187,17 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control,
 	}
 }
 
-void ActuatorEffectivenessControlSurfaces::applyScale(float scale, int first_actuator_idx,
-		ActuatorVector &actuator_sp) const
+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) *= scale;
+		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,7 +91,7 @@ class ActuatorEffectivenessControlSurfaces : public ModuleParams, public Actuato
 
 	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;
+	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

@@ -83,8 +83,8 @@ void ActuatorEffectivenessThrustVectoring::updateSetpoint(const matrix::Vector<f
 		}
 	}
 
-	constexpr float propeller_coefficient = 1.f / (20000.f * 20000.f); // scale down the thrust
-	float thrust = rpm * rpm * propeller_coefficient;
+	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) {
@@ -95,6 +95,7 @@ void ActuatorEffectivenessThrustVectoring::updateSetpoint(const matrix::Vector<f
 		}
 	}
 
-	const float scale = 1.f / fmaxf(thrust, 0.2f);
-	_control_surfaces.applyScale(scale, _first_control_surface_idx, actuator_sp);
+	const float effectiveness_scale = fmaxf(thrust, 0.2f);
+	_control_surfaces.applyEffectivenessScale(effectiveness_scale, _first_control_surface_idx, actuator_sp,
+			_actuator_effectiveness_scale);
 }