From 189bdf05f7d064f25bf91cb3880ecba80e8a4d98 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 18 Feb 2026 13:02:16 +0100 Subject: [PATCH] final piece, from allocator set actuators nan before slew limiting --- .../control_allocation/ControlAllocation.hpp | 18 ++++++++++++++++++ .../control_allocator/ControlAllocator.cpp | 4 ++++ 2 files changed, 22 insertions(+) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp index a106120026..8332d7a7ca 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -222,6 +222,24 @@ public: ActuatorVector normalizeActuatorSetpoint(const ActuatorVector &actuator) const; + /** + * Apply a mask of actuators to be set to NaN. + * + * A NaN value in _actuator_sp represents a disabled or stopped actuator. + * This mask is typically used to stop motors in specific flight phases or when certain thrust components are NaN. + * + * @param nan_actuators_mask Bitmask indicating which actuators to set to NaN. + * If (nan_actuators_mask & (1 << i)), _actuator_sp(i) becomes NaN. + */ + void applyNanToActuators(uint32_t nan_actuators_mask) + { + for (int i = 0; i < _num_actuators && i < 32; i++) { + if (nan_actuators_mask & (1u << i)) { + _actuator_sp(i) = NAN; + } + } + } + virtual void updateParameters() {} int numConfiguredActuators() const { return _num_actuators; } diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index e86db94c5a..3ab965434b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -456,6 +456,10 @@ ControlAllocator::Run() _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + if (i == 0) { + _control_allocation[i]->applyNanToActuators(_actuator_effectiveness->getStoppedMotors()); + } + if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); }