From 029edb50b33c168894b2adc7388a1f87c718ace6 Mon Sep 17 00:00:00 2001 From: Balduin Date: Mon, 9 Mar 2026 15:08:52 +0100 Subject: [PATCH] refactor(control_allocation): Clarify and enforce that motors are in matrix 0 --- .../actuator_effectiveness/ActuatorEffectiveness.cpp | 5 +++++ src/modules/control_allocator/ControlAllocator.cpp | 5 +++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 87e72a9977..b714829f04 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -50,6 +50,11 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m return -1; } + if (type == ActuatorType::MOTORS && selected_matrix != 0) { + PX4_ERR("Trying to add motors to matrix %d (add to matrix 0)", selected_matrix); + return -1; + } + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0); effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1); effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 3a6788c33c..39642bf71d 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -438,6 +438,7 @@ ControlAllocator::Run() _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); if (i == 0) { + // The motors are always in allocation 0 handle_stopped_motors(now); } @@ -611,7 +612,7 @@ ControlAllocator::handle_stopped_motors(const hrt_abstime now) | _motor_stop_mask; // Handle stopped motors by setting NaN - const unsigned int allocation_index = 0; + const unsigned int allocation_index = 0; // Motors always in allocation 0 _control_allocation[allocation_index]->applyNanToActuators(stopped_motors); // Apply ice shedding, which applies _only_ to stopped motors @@ -619,7 +620,7 @@ ControlAllocator::handle_stopped_motors(const hrt_abstime now) const float ice_shedding_output = get_ice_shedding_output(now); if (ice_shedding_output > FLT_EPSILON && !any_stopped_motor_failed) { - for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + for (int motors_idx = 0; motors_idx < _num_actuators[allocation_index] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { if (stopped_motors & 1u << motors_idx) { _control_allocation[allocation_index]->_actuator_sp(motors_idx) = ice_shedding_output; }