From 2a077181d900e32bf66cd4986e7beadad7d2c576 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 4 May 2023 15:17:43 +0200 Subject: [PATCH] ActuatorEffectiveness: add function to delectively stop motors with zero thrust --- .../ActuatorEffectiveness.cpp | 16 ++++++++++++++++ .../ActuatorEffectiveness.hpp | 13 +++++++++++-- .../ActuatorEffectivenessStandardVTOL.cpp | 4 ++-- .../ActuatorEffectivenessStandardVTOL.hpp | 4 ---- .../ActuatorEffectivenessTailsitterVTOL.cpp | 4 ++-- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 4 ++-- 6 files changed, 33 insertions(+), 12 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp index e8b2b3bea6..099adb3120 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -84,3 +84,19 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const return total_count; } + +void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) +{ + for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) { + const uint32_t motor_mask = (1u << actuator_idx); + + if (stoppable_motors_mask & motor_mask) { + if (fabsf(actuator_sp(actuator_idx)) < .02f) { + _stopped_motors_mask |= motor_mask; + + } else { + _stopped_motors_mask &= ~motor_mask; + } + } + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 1317011850..0ddd4988f3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -203,7 +203,7 @@ public: /** * Get a bitmask of motors to be stopped */ - virtual uint32_t getStoppedMotors() const { return 0; } + virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; } /** * Fill in the unallocated torque and thrust, customized by effectiveness type. @@ -211,6 +211,15 @@ public: */ virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + /** + * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero + * + * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand + * @param actuator_sp outcome of the allocation to determine if the motor should be stopped + */ + virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); + protected: - FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; + uint32_t _stopped_motors_mask{0}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 8d7fed6939..74e3ae562e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -94,13 +94,13 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = _mc_motors_mask; + _stopped_motors_mask = _mc_motors_mask; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors = 0; + _stopped_motors_mask = 0; break; } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index e49edc6176..5c9a905fd1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -77,18 +77,14 @@ public: void setFlightPhase(const FlightPhase &flight_phase) override; - uint32_t getStoppedMotors() const override { return _stopped_motors; } - private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight) - uint32_t _stopped_motors{}; ///< currently stopped motors int _first_control_surface_idx{0}; ///< applies to matrix 1 uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; - }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 139dc36612..7c2150ee9c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -100,13 +100,13 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig // update stopped motors //TODO: add option to switch off certain motors in FW switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = 0; + _stopped_motors_mask = 0; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors = 0; + _stopped_motors_mask = 0; break; } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 84695a9bb2..ccd2cc98f6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -180,13 +180,13 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = _nontilted_motors; + _stopped_motors_mask = _nontilted_motors; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors = 0; + _stopped_motors_mask = 0; break; } }