CA: reload CA matrix to newly created CA class properly

Also remove the dependency of ActuatorEffectivenessMultirotor
to param update uORB topic; the CA module
sends a "force" parameter when needed

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -77,7 +77,7 @@ public:
 	 *
 	 * @return true if updated and matrix is set
 	 */
-	virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) = 0;
+	virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) = 0;
 
 	/**
 	 * Get the actuator trims

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -41,21 +41,16 @@
 
 #include "ActuatorEffectivenessMultirotor.hpp"
 
-ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
-	ModuleParams(nullptr)
+ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent):
+	ModuleParams(parent)
 {
 }
 
 bool
-ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
+ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
+		bool force)
 {
-	// Check if parameters have changed
-	if (_updated || _parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
+	if (_updated || force) {
 		_updated = false;
 
 		// Get multirotor geometry
@@ -148,6 +143,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 	effectiveness.setZero();
 
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
+
 		// Get rotor axis
 		matrix::Vector3f axis(
 			geometry.rotors[i].axis_x,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -46,14 +46,13 @@
 #include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
 
 using namespace time_literals;
 
 class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessMultirotor();
+	ActuatorEffectivenessMultirotor(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessMultirotor() = default;
 
 	static constexpr int NUM_ROTORS_MAX = 8;
@@ -76,12 +75,10 @@ public:
 	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
 
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) override;
 
 	int numActuators() const override { return _num_actuators; }
 private:
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
 	bool _updated{true};
 	int _num_actuators{0};
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -47,9 +47,10 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
 }
 
 bool
-ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
+ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
+		bool force)
 {
-	if (!_updated) {
+	if (!(_updated || force)) {
 		return false;
 	}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -49,7 +49,7 @@ public:
 	ActuatorEffectivenessStandardVTOL();
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;
 
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) override;
 
 	/**
 	 * Set the current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -46,9 +46,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
 bool
-ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
+ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
+		bool force)
 {
-	if (!_updated) {
+	if (!(_updated || force)) {
 		return false;
 	}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -49,7 +49,7 @@ public:
 	ActuatorEffectivenessTiltrotorVTOL();
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
 
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force = false) override;
 
 	/**
 	 * Set the current flight phase

src/modules/control_allocator/ControlAllocator.cpp

@@ -132,6 +132,9 @@ ControlAllocator::parameters_updated()
 	actuator_max(14) = _param_ca_act14_max.get();
 	actuator_max(15) = _param_ca_act15_max.get();
 	_control_allocation->setActuatorMax(actuator_max);
+
+	_control_allocation->updateParameters();
+	update_effectiveness_matrix_if_needed(true);
 }
 
 void
@@ -198,7 +201,7 @@ ControlAllocator::update_effectiveness_source()
 		switch (source) {
 		case EffectivenessSource::NONE:
 		case EffectivenessSource::MULTIROTOR:
-			tmp = new ActuatorEffectivenessMultirotor();
+			tmp = new ActuatorEffectivenessMultirotor(this);
 			break;
 
 		case EffectivenessSource::STANDARD_VTOL:
@@ -245,18 +248,12 @@ ControlAllocator::Run()
 
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated()) {
-		updateParams();
-		parameters_updated();
-
-		if (_control_allocation) {
-			_control_allocation->updateParameters();
-		}
-
-		update_effectiveness_matrix_if_needed();
-
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		parameters_updated();
 	}
 
 	if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) {
@@ -351,11 +348,12 @@ ControlAllocator::Run()
 }
 
 void
-ControlAllocator::update_effectiveness_matrix_if_needed()
+ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
 {
 	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness;
 
-	if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) {
+	if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness, force)) {
+
 		const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
 
 		// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)

src/modules/control_allocator/ControlAllocator.hpp

@@ -105,7 +105,7 @@ private:
 	void update_allocation_method();
 	void update_effectiveness_source();
 
-	void update_effectiveness_matrix_if_needed();
+	void update_effectiveness_matrix_if_needed(bool force = false);
 
 	void publish_actuator_setpoint();
 	void publish_control_allocator_status();