From c6a1adf8cddc5e9a00dfa3d7be6c6878ce5bffb0 Mon Sep 17 00:00:00 2001 From: Balduin Date: Tue, 10 Mar 2026 08:33:38 +0100 Subject: [PATCH] feat(control_allocator): Stop motors on NaN thrust setpoint - Separate stopped motor mask into separate state variables (one for stopping due to flight phase, one for stopping due to NaN thrust) and combine them on demand in getStoppedMotors. This removes the need of hard to follow &= and |= operation involving shared state. - Introduce forwards/upwards/sideways motor masks in base ActuatorEffecctiveness (and remove previous from individual Effectiveness classes), and introduce getSidewaysMotors function - Set the masks in the relevant VehicleActuatorEffectiveness classes - add setStoppedMotorsBasedOnThrust, which sets the appropriate state variables and is called from ControlAllocator - use the new nanToZero to clean _thrust_sp, for CA internal use --- .../ActuatorEffectiveness.cpp | 19 ++++++++++++ .../ActuatorEffectiveness.hpp | 29 +++++++++++++++++-- .../control_allocator/ControlAllocator.cpp | 3 ++ .../ActuatorEffectivenessFixedWing.hpp | 2 -- .../ActuatorEffectivenessMCTilt.cpp | 4 +++ .../ActuatorEffectivenessMultirotor.cpp | 4 +++ .../ActuatorEffectivenessRotors.cpp | 21 ++++++++++++++ .../ActuatorEffectivenessRotors.hpp | 2 ++ .../ActuatorEffectivenessStandardVTOL.cpp | 5 ++-- .../ActuatorEffectivenessStandardVTOL.hpp | 3 -- .../ActuatorEffectivenessTailsitterVTOL.cpp | 19 ++++++++++-- .../ActuatorEffectivenessTailsitterVTOL.hpp | 4 +-- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 7 +++-- 13 files changed, 104 insertions(+), 18 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 05ffb097f2..1d887a5f3f 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -88,3 +88,22 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const return total_count; } + +ActuatorEffectiveness::ActuatorBitmask ActuatorEffectiveness::getStoppedMotors() const +{ + // Motors can be stopped (here) for two reasons. They can + // additionally be stopped in the ControlAllocator, due to motor + // failure. + + // a) they are turned off because they are generally not used in the given flight phase. + ActuatorEffectiveness::ActuatorBitmask 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; } + + if (_sideways_motors_stopped_by_thrust) { stopped_motors_mask |= _sideways_motors_mask; } + + return stopped_motors_mask; +} diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 30d9b415f0..867da079cf 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -205,7 +205,7 @@ public: /** * Get a bitmask of motors to be stopped */ - virtual ActuatorBitmask getStoppedMotors() const { return _stopped_motors_mask; } + ActuatorBitmask getStoppedMotors() const; /** * Fill in the unallocated torque and thrust, customized by effectiveness type. @@ -213,8 +213,33 @@ public: */ virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + /** + * Record which components of the thrust setpoint are NaN, so that motors in that direction are stopped. + * + * @param thrust_sp The thrust setpoint as received by the allocator (instance zero - multicopter only) + */ + void stopMotorsBasedOnThrustSetpoint(const matrix::Vector3f &thrust_sp) + { + _forwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(0)); + _sideways_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(1)); + _upwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(2)); + } + protected: + + // The vehicle effectiveness classes should set these to + // ActuatorEffectivenessRotors::get{Upwards,Forwards,Sideways}Motors() + // They can alternatively be set to zero to completely disable stopping motors based on NaN thrust + + ActuatorBitmask _upwards_motors_mask{}; + ActuatorBitmask _forwards_motors_mask{}; + ActuatorBitmask _sideways_motors_mask{}; + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; - ActuatorBitmask _stopped_motors_mask{0}; + ActuatorBitmask _stopped_motors_mask_due_to_flight_phase{}; + + bool _forwards_motors_stopped_by_thrust{false}; + bool _upwards_motors_stopped_by_thrust{false}; + bool _sideways_motors_stopped_by_thrust{false}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 39642bf71d..842b6379c5 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -395,6 +395,9 @@ ControlAllocator::Run() if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + + _actuator_effectiveness->stopMotorsBasedOnThrustSetpoint(_thrust_sp); + _thrust_sp.nanToZero(); } if (do_update) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index cc955d1acc..e4448db256 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -64,6 +64,4 @@ private: uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; int _first_control_surface_idx{0}; ///< applies to matrix 1 - - ActuatorBitmask _forwards_motors_mask{}; }; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 984ab00f9d..f9f3c5948f 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -59,6 +59,10 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration _tilts.updateTorqueSign(_mc_rotors.geometry()); const bool tilts_added_successfully = _tilts.addActuators(configuration); + _forwards_motors_mask = _mc_rotors.getForwardsMotors(); + _sideways_motors_mask = _mc_rotors.getSidewaysMotors(); + _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + // Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle). // Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated // and reduced to 0 with the sequential desaturation method. Instead we add it after. diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index 41ed5cb2c5..2ead33b5c8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -52,5 +52,9 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configura // Motors const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); + _forwards_motors_mask = _mc_rotors.getForwardsMotors(); + _sideways_motors_mask = _mc_rotors.getSidewaysMotors(); + _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + return rotors_added_successfully; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 2d3ed43f08..4596d0f361 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -267,6 +267,11 @@ ActuatorBitmask ActuatorEffectivenessRotors::getMotors() const return motors; } +// These three allow an angle deviation of 8.1 deg. Motor tilt for yaw actuation +// could reasonably be more. Alternatively, we could classify the motors such +// that every rotor points in a defined direction, putting the decision +// boundaries at 45 deg. TODO revisit + ActuatorBitmask ActuatorEffectivenessRotors::getUpwardsMotors() const { ActuatorBitmask upwards_motors = 0; @@ -297,6 +302,22 @@ ActuatorBitmask ActuatorEffectivenessRotors::getForwardsMotors() const return forward_motors; } +ActuatorBitmask ActuatorEffectivenessRotors::getSidewaysMotors() const +{ + ActuatorBitmask sideways_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + // This includes both left and right pointing motors, in contrast to the other two directions... + if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) > 0.5f && fabsf(axis(2)) < 0.1f) { + sideways_motors |= 1u << i; + } + } + + return sideways_motors; +} + bool ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 60d9e11973..7aad2a62cd 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -129,8 +129,10 @@ public: void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } ActuatorBitmask getMotors() const; + ActuatorBitmask getUpwardsMotors() const; ActuatorBitmask getForwardsMotors() const; + ActuatorBitmask getSidewaysMotors() const; private: void updateParams() override; 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 4cfd663de9..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; - ActuatorBitmask _upwards_motors_mask{}; - ActuatorBitmask _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..a7f1293809 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -59,6 +59,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi // enable MC yaw control if more than 3 rotors _mc_rotors.enableYawByDifferentialThrust(_mc_rotors.geometry().num_rotors > 3); const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); + updateMotorMasks(); // Control Surfaces configuration.selected_matrix = 1; @@ -96,17 +97,29 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors - switch (flight_phase) { + updateMotorMasks(); + +} + +void ActuatorEffectivenessTailsitterVTOL::updateMotorMasks() +{ + switch (_flight_phase) { case FlightPhase::FORWARD_FLIGHT: + + // TODO only set this mask if the motors are not needed for rate control, watching one or both of: + // - _mc_rotors.geometry().num_rotors > 3 (used in getEffectivenessMatrix to enable MC yaw control) + // - VT_FW_DIFTHR_EN (the general switch for this -- are the two equivalent?) + _forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards + _upwards_motors_mask = 0; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: _forwards_motors_mask = 0; - _stopped_motors_mask = 0; + _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); + break; } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 0429b11300..6574955516 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -77,11 +77,11 @@ public: const char *name() const override { return "VTOL Tailsitter"; } protected: + void updateMotorMasks(); + ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; - ActuatorBitmask _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/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index fb3eb541cb..d59a93458f 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -83,6 +83,8 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config << configuration.num_actuators[(int)ActuatorType::MOTORS]; const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); + _forwards_motors_mask = _mc_rotors.getForwardsMotors(); + _upwards_motors_mask = _mc_rotors.getUpwardsMotors(); _motors = _mc_rotors.getMotors(); // Control Surfaces @@ -216,16 +218,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; } }