control_allocation: introduced ControlAllocationPseudoInverse class

which serves as base for allocation algorithms using pseudo inverse

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/control_allocator/ControlAllocation/CMakeLists.txt

@@ -34,7 +34,7 @@
 px4_add_library(ControlAllocation
 	ControlAllocation.cpp
 	ControlAllocationMultirotor.cpp
-	ControlAllocationSimple.cpp
+	ControlAllocationPseudoInverse.cpp
 	ControlAllocationSequentialDesaturation.cpp
 )
 
@@ -45,5 +45,5 @@ target_include_directories(ControlAllocation
 
 target_link_libraries(ControlAllocation PRIVATE mathlib)
 
-px4_add_unit_gtest(SRC ControlAllocationSimpleTest.cpp LINKLIBS ControlAllocation)
+px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
 # px4_add_unit_gtest(SRC ControlAllocationMultirotorTest.cpp LINKLIBS ControlAllocation)

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -70,7 +70,6 @@ ControlAllocation::setEffectivenessMatrix(const
 		matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &B)
 {
 	_B = B;
-	_A_update_needed = true;
 }
 
 const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -205,7 +205,4 @@ class ControlAllocation
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_sp;  	//< Actuator setpoint
 	matrix::Vector<float, NUM_AXES> _control_sp;   		//< Control setpoint
 	matrix::Vector<float, NUM_AXES> _control_allocated;  	//< Allocated control
-
-	bool _A_update_needed{false};
-
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationMultirotor.cpp

@@ -59,17 +59,16 @@ ControlAllocationMultirotor::allocate()
 {
 	// Update mixer if needed
 	if (_mixer == nullptr) {
-		// Compute pseudoinverse of effectiveness matrix
-		matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> A = matrix::geninv(_B);
+		ControlAllocationPseudoInverse::updatePseudoInverse();
 
 		// Convert A to MultirotorMixer::Rotor
 		MultirotorMixer::Rotor rotors[NUM_ACTUATORS];
 
 		for (size_t i = 0; i < NUM_ACTUATORS; i++) {
-			rotors[i].roll_scale = A(i, 0);
-			rotors[i].pitch_scale = A(i, 1);
-			rotors[i].yaw_scale = A(i, 2);
-			rotors[i].thrust_scale = -A(i, 5); // -Z thrust
+			rotors[i].roll_scale = _A(i, 0);
+			rotors[i].pitch_scale = _A(i, 1);
+			rotors[i].yaw_scale = _A(i, 2);
+			rotors[i].thrust_scale = -_A(i, 5); // -Z thrust
 		}
 
 		_mixer = new MultirotorMixer(mixer_callback, (uintptr_t)this, rotors, NUM_ACTUATORS);

src/modules/control_allocator/ControlAllocation/ControlAllocationMultirotor.hpp

@@ -45,12 +45,12 @@
 
 #pragma once
 
-#include "ControlAllocation.hpp"
+#include "ControlAllocationPseudoInverse.hpp"
 
 #define MIXER_MULTIROTOR_USE_MOCK_GEOMETRY
 #include <lib/mixer/mixer.h>
 
-class ControlAllocationMultirotor: public ControlAllocation
+class ControlAllocationMultirotor: public ControlAllocationPseudoInverse
 {
 public:
 	ControlAllocationMultirotor() = default;

src/modules/control_allocator/ControlAllocation/ControlAllocationSimple.cpp → src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -32,24 +32,29 @@
  ****************************************************************************/
 
 /**
- * @file ControlAllocationSimple.hpp
+ * @file ControlAllocationPseudoInverse.hpp
  *
  * Simple Control Allocation Algorithm
  *
  * @author Julien Lecoeur <julien.lecoeur@gmail.com>
  */
 
-#include "ControlAllocationSimple.hpp"
-
+#include "ControlAllocationPseudoInverse.hpp"
 
 void
-ControlAllocationSimple::allocate()
+ControlAllocationPseudoInverse::updatePseudoInverse()
 {
 	// Compute new gains if needed
 	if (_A_update_needed) {
 		_A = matrix::geninv(_B);
 		_A_update_needed = false;
 	}
+}
+
+void
+ControlAllocationPseudoInverse::allocate()
+{
+	updatePseudoInverse();
 
 	// Allocate
 	_actuator_sp = _A * _control_sp;

src/modules/control_allocator/ControlAllocation/ControlAllocationSimple.hpp → src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file ControlAllocationSimple.hpp
+ * @file ControlAllocationPseudoInverse.hpp
  *
  * Simple Control Allocation Algorithm
  *
@@ -47,14 +47,22 @@
 
 #include "ControlAllocation.hpp"
 
-class ControlAllocationSimple: public ControlAllocation
+class ControlAllocationPseudoInverse: public ControlAllocation
 {
 public:
-	ControlAllocationSimple() = default;
-	virtual ~ControlAllocationSimple() = default;
+	ControlAllocationPseudoInverse() = default;
+	virtual ~ControlAllocationPseudoInverse() = default;
 
 	void allocate() override;
 
-private:
+protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _A;
+
+	bool _A_update_needed{false};
+
+	/**
+	 * Recalculate pseudo inverse if required.
+	 *
+	 */
+	void updatePseudoInverse();
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationSimpleTest.cpp → src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

@@ -40,13 +40,13 @@
  */
 
 #include <gtest/gtest.h>
-#include <ControlAllocationSimple.hpp>
+#include <ControlAllocationPseudoInverse.hpp>
 
 using namespace matrix;
 
 TEST(ControlAllocationTest, AllZeroCase)
 {
-	ControlAllocationSimple method;
+	ControlAllocationPseudoInverse method;
 
 	matrix::Vector<float, 6> control_sp;
 	matrix::Vector<float, 6> control_allocated;

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -44,11 +44,7 @@
 void
 ControlAllocationSequentialDesaturation::allocate()
 {
-	// Compute new gains if needed
-	if (_A_update_needed) {
-		_A = matrix::geninv(_B);
-		_A_update_needed = false;
-	}
+	ControlAllocationPseudoInverse::updatePseudoInverse();
 
 	// Allocate
 	_actuator_sp = _A * _control_sp;
@@ -83,20 +79,14 @@ ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::desat
 
 }
 
-ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(ControlAxis axis)
+const ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(ControlAxis axis)
 {
-	ActuatorVector ret;
-
-	for (unsigned i = 0; i < NUM_ACTUATORS; i++) {
-		ret(i) = _A(i, axis);
-	}
-
-	return ret;
+	return _A.slice<NUM_ACTUATORS, 1>(0, axis);
 }
 
 
-float ControlAllocationSequentialDesaturation::computeDesaturationGain(ActuatorVector desaturation_vector,
-		ActuatorVector actuator_sp)
+float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
+		const ActuatorVector &actuator_sp)
 {
 	float k_min = 0.f;
 	float k_max = 0.f;

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp

@@ -43,9 +43,9 @@
 
 #pragma once
 
-#include "ControlAllocation.hpp"
+#include "ControlAllocationPseudoInverse.hpp"
 
-class ControlAllocationSequentialDesaturation: public ControlAllocation
+class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse
 {
 public:
 
@@ -55,7 +55,6 @@ class ControlAllocationSequentialDesaturation: public ControlAllocation
 	void allocate() override;
 
 private:
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _A;
 
 	/**
 	 * List of control axis used for desaturating the actuator vector. The desaturation logic will sequentially
@@ -77,7 +76,7 @@ class ControlAllocationSequentialDesaturation: public ControlAllocation
 	 * @param 	axis 			Control axis
 	 * @return 	ActuatorVector 	Column of the pseudo-inverse matrix corresponding to the given control axis.
 	 */
-	ActuatorVector getDesaturationVector(ControlAxis axis);
+	const ActuatorVector getDesaturationVector(ControlAxis axis);
 
 	/**
 	 * Compute desaturation gain.
@@ -86,5 +85,5 @@ class ControlAllocationSequentialDesaturation: public ControlAllocation
 	 * @param 	Actuator setpoint vector.
 	 * @return 	Gain which eliminates the saturation of the highest saturated actuator.
 	 */
-	float computeDesaturationGain(ActuatorVector desaturation_vector, ActuatorVector actuator_sp);
+	float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -196,7 +196,7 @@ ControlAllocator::update_allocation_method()
 
 		switch (method) {
 		case 0:
-			tmp = new ControlAllocationSimple();
+			tmp = new ControlAllocationPseudoInverse();
 			break;
 
 		case 1:
@@ -245,8 +245,8 @@ ControlAllocator::update_allocation_method()
 
 	// Guard against bad initialization
 	if (_control_allocation == nullptr) {
-		PX4_ERR("Falling back to ControlAllocationSimple");
-		_control_allocation = new ControlAllocationSimple();
+		PX4_ERR("Falling back to ControlAllocationPseudoInverse");
+		_control_allocation = new ControlAllocationPseudoInverse();
 		_allocation_method_id = 0;
 	}
 }

src/modules/control_allocator/ControlAllocator.hpp

@@ -42,7 +42,7 @@
 #pragma once
 
 #include <ControlAllocation.hpp>
-#include <ControlAllocationSimple.hpp>
+#include <ControlAllocationPseudoInverse.hpp>
 #include <ControlAllocationMultirotor.hpp>
 #include <ControlAllocationSequentialDesaturation.hpp>