From 308f614735a90e3dfea636675009842f68fa9368 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:38 +0200 Subject: [PATCH] refactor control_allocator: directly get the effectiveness matrix when updated Reduces stack + RAM usage --- .../ActuatorEffectiveness.hpp | 27 +-- .../ActuatorEffectivenessMultirotor.cpp | 179 ++++++++---------- .../ActuatorEffectivenessMultirotor.hpp | 18 +- .../ActuatorEffectivenessStandardVTOL.cpp | 34 ++-- .../ActuatorEffectivenessStandardVTOL.hpp | 9 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 37 ++-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 9 +- .../control_allocator/ControlAllocator.cpp | 57 +++--- .../control_allocator/ControlAllocator.hpp | 2 + 9 files changed, 164 insertions(+), 208 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 174a9069b3..f0ac4bcb92 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/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 &getEffectivenessMatrix() const - { - return _effectiveness; - }; + virtual bool getEffectivenessMatrix(matrix::Matrix &matrix) = 0; /** * Get the actuator trims @@ -97,7 +87,7 @@ public: const matrix::Vector &getActuatorTrim() const { return _trim; - }; + } /** * Get the current flight phase @@ -107,10 +97,9 @@ public: const FlightPhase &getFlightPhase() const { return _flight_phase; - }; + } protected: - matrix::Matrix _effectiveness; //< Effectiveness matrix - matrix::Vector _trim; //< Actuator trim - FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; //< Current flight phase + matrix::Vector _trim; ///< Actuator trim + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index a486422193..a30ac2fde6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -44,35 +44,105 @@ ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): ModuleParams(nullptr) { - parameters_updated(); } bool -ActuatorEffectivenessMultirotor::update() +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &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(¶m_update); updateParams(); - parameters_updated(); + _updated = false; - updated = true; + // Get multirotor geometry + MultirotorGeometry geometry = {}; + geometry.rotors[0].position_x = _param_ca_mc_r0_px.get(); + geometry.rotors[0].position_y = _param_ca_mc_r0_py.get(); + geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get(); + geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); + geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); + geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); + geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get(); + geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get(); + + geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); + geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); + geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get(); + geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); + geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); + geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); + geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get(); + geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get(); + + geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); + geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); + geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get(); + geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); + geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); + geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); + geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get(); + geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get(); + + geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); + geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); + geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get(); + geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); + geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); + geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); + geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get(); + geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get(); + + geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); + geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); + geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get(); + geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); + geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); + geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); + geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get(); + geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get(); + + geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); + geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); + geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get(); + geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); + geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); + geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); + geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get(); + geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get(); + + geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); + geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); + geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get(); + geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); + geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); + geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); + geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get(); + geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get(); + + geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); + geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); + geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get(); + geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); + geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); + geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); + geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get(); + geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get(); + + computeEffectivenessMatrix(geometry, matrix); + return true; } - return updated; + return false; } -matrix::Matrix -ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry) +void +ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness) { - matrix::Matrix - effectiveness; - for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { // Get rotor axis matrix::Vector3f axis( @@ -115,87 +185,4 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry g effectiveness(j + 3, i) = thrust(j); } } - - return effectiveness; -} - -void -ActuatorEffectivenessMultirotor::parameters_updated() -{ - // Get multirotor geometry - MultirotorGeometry geometry = {}; - geometry.rotors[0].position_x = _param_ca_mc_r0_px.get(); - geometry.rotors[0].position_y = _param_ca_mc_r0_py.get(); - geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get(); - geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); - geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); - geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); - geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get(); - geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get(); - - geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); - geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); - geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get(); - geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); - geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); - geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); - geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get(); - geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get(); - - geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); - geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); - geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get(); - geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); - geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); - geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); - geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get(); - geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get(); - - geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); - geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); - geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get(); - geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); - geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); - geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); - geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get(); - geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get(); - - geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); - geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); - geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get(); - geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); - geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); - geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); - geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get(); - geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get(); - - geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); - geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); - geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get(); - geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); - geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); - geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); - geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get(); - geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get(); - - geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); - geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); - geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get(); - geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); - geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); - geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); - geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get(); - geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get(); - - geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); - geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); - geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get(); - geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); - geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); - geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); - 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); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 1b07a46586..c5be27fc91 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/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 computeEffectivenessMatrix(MultirotorGeometry); + static void computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness); + + bool getEffectivenessMatrix(matrix::Matrix &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) _param_ca_mc_r0_px, (ParamFloat) _param_ca_mc_r0_py, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 8acde43c9d..c5b2b8bb75 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -47,23 +47,12 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() } bool -ActuatorEffectivenessStandardVTOL::update() +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) { - if (_updated) { - _updated = false; - return true; + if (!_updated) { + return false; } - return false; -} - -void -ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) -{ - ActuatorEffectiveness::setFlightPhase(flight_phase); - - _updated = true; - switch (_flight_phase) { case FlightPhase::HOVER_FLIGHT: { const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { @@ -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(standard_vtol); + matrix = matrix::Matrix(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(standard_vtol); + matrix = matrix::Matrix(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(standard_vtol); + matrix = matrix::Matrix(standard_vtol); break; } } + + _updated = false; + return true; +} + +void +ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + _updated = true; + } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index dd36b54481..01cff0a764 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/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 &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}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 1b69a8789d..c2001386a9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -45,25 +45,13 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() { setFlightPhase(FlightPhase::HOVER_FLIGHT); } - bool -ActuatorEffectivenessTiltrotorVTOL::update() +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) { - if (_updated) { - _updated = false; - return true; + if (!_updated) { + return false; } - 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(tiltrotor_vtol); + matrix = matrix::Matrix(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; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index f2a9832703..d2f0633341 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/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 &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}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 192ab56fa1..eaa2647069 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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 effectiveness = _actuator_effectiveness->getEffectivenessMatrix(); - matrix::Vector 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 effectiveness = _actuator_effectiveness->getEffectivenessMatrix(); - matrix::Vector trim = _actuator_effectiveness->getActuatorTrim(); - - // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) - matrix::Vector actuator_max = _control_allocation->getActuatorMax(); - matrix::Vector 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 c; @@ -374,6 +344,31 @@ ControlAllocator::Run() perf_end(_loop_perf); } +void +ControlAllocator::update_effectiveness_matrix_if_needed() +{ + matrix::Matrix effectiveness; + + if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) { + const matrix::Vector &trim = _actuator_effectiveness->getActuatorTrim(); + + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + matrix::Vector 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() { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 13a5894a36..9a10454012 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/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();