refactor control_allocator: directly get the effectiveness matrix when updated

Reduces stack + RAM usage

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -62,13 +62,6 @@ public:
 		TRANSITION_FF_TO_HF = 3
 	};
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() = 0;
-
 	/**
 	 * Set the current flight phase
 	 *
@@ -77,17 +70,14 @@ public:
 	virtual void setFlightPhase(const FlightPhase &flight_phase)
 	{
 		_flight_phase = flight_phase;
-	};
+	}
 
 	/**
-	 * Get the control effectiveness matrix
+	 * Get the control effectiveness matrix if updated
 	 *
-	 * @return Effectiveness matrix
+	 * @return true if updated and matrix is set
 	 */
-	const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const
-	{
-		return _effectiveness;
-	};
+	virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) = 0;
 
 	/**
 	 * Get the actuator trims
@@ -97,7 +87,7 @@ public:
 	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
 	{
 		return _trim;
-	};
+	}
 
 	/**
 	 * Get the current flight phase
@@ -107,10 +97,9 @@ public:
 	const FlightPhase &getFlightPhase() const
 	{
 		return _flight_phase;
-	};
+	}
 
 protected:
-	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  //< Effectiveness matrix
-	matrix::Vector<float, NUM_ACTUATORS> _trim;			//< Actuator trim
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		//< Current flight phase
+	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -44,84 +44,20 @@
 ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
 	ModuleParams(nullptr)
 {
-	parameters_updated();
 }
 
 bool
-ActuatorEffectivenessMultirotor::update()
+ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
 {
-	bool updated = false;
-
 	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
+	if (_updated || _parameter_update_sub.updated()) {
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
 
 		updateParams();
-		parameters_updated();
-
-		updated = true;
-	}
-
-	return updated;
-}
-
-matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
-ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry)
-{
-	matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
-	effectiveness;
-
-	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
-		// Get rotor axis
-		matrix::Vector3f axis(
-			geometry.rotors[i].axis_x,
-			geometry.rotors[i].axis_y,
-			geometry.rotors[i].axis_z
-		);
-
-		// Normalize axis
-		float axis_norm = axis.norm();
-
-		if (axis_norm > FLT_EPSILON) {
-			axis /= axis_norm;
-
-		} else {
-			// Bad axis definition, ignore this rotor
-			continue;
-		}
-
-		// Get rotor position
-		matrix::Vector3f position(
-			geometry.rotors[i].position_x,
-			geometry.rotors[i].position_y,
-			geometry.rotors[i].position_z
-		);
-
-		// Get coefficients
-		float ct = geometry.rotors[i].thrust_coef;
-		float km = geometry.rotors[i].moment_ratio;
-
-		// Compute thrust generated by this rotor
-		matrix::Vector3f thrust = ct * axis;
-
-		// Compute moment generated by this rotor
-		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
-
-		// Fill corresponding items in effectiveness matrix
-		for (size_t j = 0; j < 3; j++) {
-			effectiveness(j, i) = moment(j);
-			effectiveness(j + 3, i) = thrust(j);
-		}
-	}
-
-	return effectiveness;
-}
+		_updated = false;
 
-void
-ActuatorEffectivenessMultirotor::parameters_updated()
-{
 		// Get multirotor geometry
 		MultirotorGeometry geometry = {};
 		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
@@ -196,6 +132,57 @@ ActuatorEffectivenessMultirotor::parameters_updated()
 		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
 		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
 
-	// Compute effectiveness matrix
-	_effectiveness = computeEffectivenessMatrix(geometry);
+		computeEffectivenessMatrix(geometry, matrix);
+		return true;
+	}
+
+	return false;
+}
+
+void
+ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+		matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
+{
+	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
+		// Get rotor axis
+		matrix::Vector3f axis(
+			geometry.rotors[i].axis_x,
+			geometry.rotors[i].axis_y,
+			geometry.rotors[i].axis_z
+		);
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		matrix::Vector3f position(
+			geometry.rotors[i].position_x,
+			geometry.rotors[i].position_y,
+			geometry.rotors[i].position_z
+		);
+
+		// Get coefficients
+		float ct = geometry.rotors[i].thrust_coef;
+		float km = geometry.rotors[i].moment_ratio;
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i) = moment(j);
+			effectiveness(j + 3, i) = thrust(j);
+		}
+	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -53,13 +53,6 @@ public:
 	ActuatorEffectivenessMultirotor();
 	virtual ~ActuatorEffectivenessMultirotor() = default;
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() override;
-
 	static constexpr int NUM_ROTORS_MAX = 8;
 
 	typedef struct {
@@ -77,16 +70,17 @@ public:
 		RotorGeometry rotors[NUM_ROTORS_MAX];
 	} MultirotorGeometry;
 
-	static matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> computeEffectivenessMatrix(MultirotorGeometry);
+	static void computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+					       matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
+
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
 private:
-	/**
-	 * initialize some vectors/matrices from parameters
-	 */
-	void parameters_updated();
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
 
+	bool _updated{true};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
 		(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -47,22 +47,11 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
 }
 
 bool
-ActuatorEffectivenessStandardVTOL::update()
+ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
 {
-	if (_updated) {
-		_updated = false;
-		return true;
-	}
-
+	if (!_updated) {
 		return false;
-}
-
-void
-ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-
-	_updated = true;
+	}
 
 	switch (_flight_phase) {
 	case FlightPhase::HOVER_FLIGHT:  {
@@ -74,7 +63,7 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas
 				{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 				{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 			};
-			_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
+			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
 			break;
 		}
 
@@ -87,7 +76,7 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas
 				{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 				{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 			};
-			_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
+			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
 			break;
 		}
 
@@ -101,8 +90,19 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas
 				{  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 				{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 			};
-			_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
+			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
 			break;
 		}
 	}
+
+	_updated = false;
+	return true;
+}
+
+void
+ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+	_updated = true;
+
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -49,12 +49,7 @@ public:
 	ActuatorEffectivenessStandardVTOL();
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
 	/**
 	 * Set the current flight phase
@@ -64,5 +59,5 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 protected:
-	bool _updated{false};
+	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -45,24 +45,12 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
 {
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
-
 bool
-ActuatorEffectivenessTiltrotorVTOL::update()
+ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
 {
-	if (_updated) {
-		_updated = false;
-		return true;
-	}
-
+	if (!_updated) {
 		return false;
-}
-
-void
-ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
-{
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-
-	_updated = true;
+	}
 
 	// Trim
 	float tilt = 0.0f;
@@ -104,13 +92,24 @@ ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_pha
 		{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
 		{-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
 	};
-	_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
+	matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
 
 	// Temporarily disable a few controls (WIP)
 	for (size_t j = 4; j < 8; j++) {
-		_effectiveness(3, j) = 0.0f;
-		_effectiveness(4, j) = 0.0f;
-		_effectiveness(5, j) = 0.0f;
+		matrix(3, j) = 0.0f;
+		matrix(4, j) = 0.0f;
+		matrix(5, j) = 0.0f;
 	}
 
+
+	_updated = false;
+	return true;
+}
+
+void
+ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+
+	_updated = true;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -49,12 +49,7 @@ public:
 	ActuatorEffectivenessTiltrotorVTOL();
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
 
-	/**
-	 * Update effectiveness matrix
-	 *
-	 * @return True if the effectiveness matrix has changed
-	 */
-	virtual bool update() override;
+	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
 	/**
 	 * Set the current flight phase
@@ -64,5 +59,5 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 protected:
-	bool _updated{false};
+	bool _updated{true};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -132,13 +132,6 @@ ControlAllocator::parameters_updated()
 	actuator_max(14) = _param_ca_act14_max.get();
 	actuator_max(15) = _param_ca_act15_max.get();
 	_control_allocation->setActuatorMax(actuator_max);
-
-	// Get actuator effectiveness and trim
-	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness = _actuator_effectiveness->getEffectivenessMatrix();
-	matrix::Vector<float, NUM_ACTUATORS> trim = _actuator_effectiveness->getActuatorTrim();
-
-	// Assign control effectiveness matrix
-	_control_allocation->setEffectivenessMatrix(effectiveness, trim);
 }
 
 void
@@ -323,30 +316,7 @@ ControlAllocator::Run()
 	if (do_update) {
 		_last_run = now;
 
-		// Update effectiveness matrix if needed
-		if (_actuator_effectiveness->update()) {
-			matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness = _actuator_effectiveness->getEffectivenessMatrix();
-			matrix::Vector<float, NUM_ACTUATORS> trim = _actuator_effectiveness->getActuatorTrim();
-
-			// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
-			matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
-			matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
-
-			for (size_t j = 0; j < NUM_ACTUATORS; j++) {
-				if (actuator_min(j) >= actuator_max(j)) {
-					for (size_t i = 0; i < NUM_AXES; i++) {
-						effectiveness(i, j) = 0.0f;
-					}
-
-				}
-			}
-
-			// Assign control effectiveness matrix
-			if ((effectiveness - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) {
-				_control_allocation->setEffectivenessMatrix(effectiveness, trim);
-			}
-
-		}
+		update_effectiveness_matrix_if_needed();
 
 		// Set control setpoint vector
 		matrix::Vector<float, NUM_AXES> c;
@@ -374,6 +344,31 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+void
+ControlAllocator::update_effectiveness_matrix_if_needed()
+{
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness;
+
+	if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) {
+		const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
+
+		// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
+		matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
+		matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
+
+		for (size_t j = 0; j < NUM_ACTUATORS; j++) {
+			if (actuator_min(j) >= actuator_max(j)) {
+				for (size_t i = 0; i < NUM_AXES; i++) {
+					effectiveness(i, j) = 0.0f;
+				}
+			}
+		}
+
+		// Assign control effectiveness matrix
+		_control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators());
+	}
+}
+
 void
 ControlAllocator::publish_actuator_setpoint()
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -105,6 +105,8 @@ private:
 	void update_allocation_method();
 	void update_effectiveness_source();
 
+	void update_effectiveness_matrix_if_needed();
+
 	void publish_actuator_setpoint();
 	void publish_control_allocator_status();