diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index bd6ea7af11..12b3946efb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/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 &matrix) = 0; + virtual bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force = false) = 0; /** * Get the actuator trims diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index 11cab60aa0..8975ea7b05 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/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 &matrix) +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &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(¶m_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, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 2997f93926..378af51c61 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -46,14 +46,13 @@ #include #include #include -#include 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 &effectiveness); - bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + bool getEffectivenessMatrix(matrix::Matrix &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}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index c5b2b8bb75..beb36b952e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -47,9 +47,10 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() } bool -ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix, + bool force) { - if (!_updated) { + if (!(_updated || force)) { return false; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 135dc976df..99523cac68 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -49,7 +49,7 @@ public: ActuatorEffectivenessStandardVTOL(); virtual ~ActuatorEffectivenessStandardVTOL() = default; - bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force = false) override; /** * Set the current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index c2001386a9..d3baed187b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -46,9 +46,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() setFlightPhase(FlightPhase::HOVER_FLIGHT); } bool -ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix, + bool force) { - if (!_updated) { + if (!(_updated || force)) { return false; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 3c266b833c..41efd83939 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -49,7 +49,7 @@ public: ActuatorEffectivenessTiltrotorVTOL(); virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; - bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force = false) override; /** * Set the current flight phase diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index c8bbc95ec0..dc7991ebb9 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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(¶m_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 effectiveness; - if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) { + if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness, force)) { + const matrix::Vector &trim = _actuator_effectiveness->getActuatorTrim(); // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index f46570f3e2..7f9b61db76 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/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();