[TO REVERT] Do not autostart control_allocator, disable geninv, disab…

…le allocation method update
PX4 · Nov 8, 2019 · 4646f2a · 4646f2a
1 parent 50f46b1
commit 4646f2a
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 4 deletions.
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -61,7 +61,7 @@ mc_pos_control start
 #
 # Start Control Allocator
 #
-control_allocator start
+#control_allocator start
 
 #
 # Start Multicopter Land Detector.

diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationMultirotor.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationMultirotor.cpp
@@ -47,7 +47,26 @@ ControlAllocationMultirotor::setEffectivenessMatrix(const matrix::Matrix<float,
 	_B = B;
 
 	// Pseudo-inverse
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> A = matrix::geninv(_B);
+	// matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> A = matrix::geninv(_B);
+	const float A_quad_w[16][6] = {
+		{ -0.495383f,  0.707107f,  0.765306f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f, -0.707107f,  1.000000f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f,  0.707107f, -0.765306f,  0.0f, 0.0f, -1.000000f },
+		{ -0.495383f, -0.707107f, -1.000000f,  0.0f, 0.0f, -1.000000f },
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
+	};
+	matrix::Matrix<float, 16, 6> A = matrix::Matrix<float, 16, 6>(A_quad_w);
 
 	// Convert A to MultirotorMixer::Rotor
 	MultirotorMixer::Rotor rotors[NUM_ACTUATORS];

diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSimple.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSimple.cpp
@@ -45,7 +45,8 @@ void
 ControlAllocationSimple::setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &B)
 {
 	_B = B;
-	_A = matrix::geninv(_B);
+
+	// _A = matrix::geninv(_B);
 
 	// TODO: measure exec time for pseudo-inverse of full 6x16 matrix, and compare with
 	// faster alternative below (possible only if number of actuators is known)
@@ -56,6 +57,26 @@ ControlAllocationSimple::setEffectivenessMatrix(const matrix::Matrix<float, NUM_
 	// - create a smaller matrix with the non-0 columns
 	// - pseudo inverse
 	// - fill A by rows with pseudo inverse of non-zero columns
+
+	const float A_quad_w[16][6] = {
+		{ -0.495383f,  0.707107f,  0.765306f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f, -0.707107f,  1.000000f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f,  0.707107f, -0.765306f,  0.0f, 0.0f, -1.000000f },
+		{ -0.495383f, -0.707107f, -1.000000f,  0.0f, 0.0f, -1.000000f },
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
+	};
+	_A = matrix::Matrix<float, 16, 6>(A_quad_w);
 }
 
 

diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
@@ -86,7 +86,7 @@ ControlAllocator::parameters_updated()
 {
 	// Allocation method
 	// Do this first: in case a new method is loaded, it will be configured below
-	update_allocation_method();
+	// update_allocation_method();
 
 	// Minimum actuator values
 	matrix::Vector<float, NUM_ACTUATORS> actuator_min;