From 9cf211f48c711fe92e0a456697a52a5c3c802599 Mon Sep 17 00:00:00 2001 From: Balduin Date: Tue, 17 Feb 2026 11:52:10 +0100 Subject: [PATCH] Allocator: refactor stopped motor mask The stopped motor mask is currently a shared piece of state that is used for two purposes: - stopping motors due to the flight phase - stopping motors due to low thrust (removed in previous commit) To handle the shared state more neatly we store the two conditions that stop motors separately: - For stopping motors because they are not used in the current flight phase, the bitmask _stopped_motors_mask_due_to_flight_phase is used. By default it is 0, and individual ActuatorEffectiveness* classes may update it in setFlightPhase - For stopping motors due to NaN thrust setpoint, we have the flags _{forwards,upwards}_motors_stopped_by_thrust with their setters - in getStoppedMotors, the two are combined to form the overall rotors stopped mask By separating the reasons like this we remove the need of carrying over state in the different setFlightPhase methods (&=, |=). --- .../ActuatorEffectiveness.hpp | 41 ++++++++++++++++++- .../control_allocator/ControlAllocator.cpp | 20 ++++++++- .../ActuatorEffectivenessStandardVTOL.cpp | 5 +-- .../ActuatorEffectivenessStandardVTOL.hpp | 3 -- .../ActuatorEffectivenessTailsitterVTOL.cpp | 1 - .../ActuatorEffectivenessTiltrotorVTOL.cpp | 5 +-- 6 files changed, 62 insertions(+), 13 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 0bc2d0cf58..a91b80cbbb 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -201,7 +201,28 @@ public: /** * Get a bitmask of motors to be stopped */ - virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; } + uint32_t getStoppedMotors() const + { + // Motors can be stopped (here) for two reasons: + // a) they are turned off because they are generally not used in the given flight phase. + uint32_t stopped_motors_mask = _stopped_motors_mask_due_to_flight_phase; + + // b) they are stopped because the thrust setpoint in a given direction is NaN + if (_forwards_motors_stopped_by_thrust) { + stopped_motors_mask |= _forwards_motors_mask; + } + + if (_upwards_motors_stopped_by_thrust) { + stopped_motors_mask |= _upwards_motors_mask; + } + + // They can also be stopped in the ControlAllocator, due to + // motor failure. The difference is that the stopped mask in the + // allocator applies after the slew rate, and the mask here + // applies before. + + return stopped_motors_mask; + } /** * Fill in the unallocated torque and thrust, customized by effectiveness type. @@ -209,7 +230,23 @@ public: */ virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + virtual void setForwardsMotorsStoppedByThrust(bool stopped) + { + _forwards_motors_stopped_by_thrust = stopped; + } + + virtual void setUpwardsMotorsStoppedByThrust(bool stopped) + { + _upwards_motors_stopped_by_thrust = stopped; + } + protected: + uint32_t _upwards_motors_mask{}; + uint32_t _forwards_motors_mask{}; + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; - uint32_t _stopped_motors_mask{0}; + uint32_t _stopped_motors_mask_due_to_flight_phase{0}; + + bool _forwards_motors_stopped_by_thrust{false}; + bool _upwards_motors_stopped_by_thrust{false}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 32692f1154..e86db94c5a 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -395,7 +395,25 @@ ControlAllocator::Run() if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); - // TODO: stop the motors on axes that have NAN + + // As defined in VehicleThrustSetpoint.msg, a thrust of NaN means stopping the motor. + // No sideways motors implemented for now... + const bool stop_forwards_motors = std::isnan(_thrust_sp(0)); + const bool stop_upwards_motors = std::isnan(_thrust_sp(2)); + + // also only if it is actually about motors.... + _actuator_effectiveness->setForwardsMotorsStoppedByThrust(stop_forwards_motors); + _actuator_effectiveness->setUpwardsMotorsStoppedByThrust(stop_upwards_motors); + + // Now that the intent of stopping motors is stored, transform + // NaN to 0 to represent physical thrust again + if (stop_forwards_motors) { + _thrust_sp(0) = 0.f; + } + + if (stop_upwards_motors) { + _thrust_sp(2) = 0.f; + } } if (do_update) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 063dcc00aa..35eca10ed8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -91,16 +91,15 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors_mask |= _upwards_motors_mask; + _stopped_motors_mask_due_to_flight_phase = _upwards_motors_mask; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors_mask &= ~_upwards_motors_mask; + _stopped_motors_mask_due_to_flight_phase = 0; break; } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index cebcdf97b6..6bbadfd388 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -81,9 +81,6 @@ private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; - uint32_t _upwards_motors_mask{}; - uint32_t _forwards_motors_mask{}; - int _first_control_surface_idx{0}; ///< applies to matrix 1 uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index e908ab06b8..6e3ae3d4b4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -106,7 +106,6 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: _forwards_motors_mask = 0; - _stopped_motors_mask = 0; break; } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index fb3eb541cb..8f340ee778 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -216,16 +216,15 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors_mask |= _untiltable_motors; + _stopped_motors_mask_due_to_flight_phase = _untiltable_motors; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors_mask = 0; + _stopped_motors_mask_due_to_flight_phase = 0; break; } }