refactor(control_allocation): Clarify and enforce that motors are in matrix 0

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

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