fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

final piece, from allocator set actuators nan before slew limiting

Browse Source
This commit is contained in:
Balduin 2026-02-18 13:02:16 +01:00
parent 0fe15801b9
commit 189bdf05f7
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/lib/control_allocation/control_allocation/ControlAllocation.hpp
View File

@ -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; }

4
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -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);
}