control_allocator: major refactoring & additions

- allow effectiveness matrix to select control allocator method (desaturation algorithm) - add actuator_servos publication - add support for multiple matrices (for vtol) - add updateSetpoint callback method to actuator effectiveness to allow it to manipulate the actuator setpoint after allocation - handle motor stopping & reversal - add control surfaces & tilt servos - handle standard vtol + tiltrotor - rename MC rotors params & class to be more generically usable - fixes and enables ActuatorEffectivenessRotorsTest
2026-07-01 21:20:35 +08:00 · 2021-11-24 15:12:09 +01:00
parent a81f11acdd
commit 70e46a194f
28 changed files with 1936 additions and 775 deletions
@@ -59,7 +59,10 @@ ControlAllocator::ControlAllocator() :

 ControlAllocator::~ControlAllocator()
 {
-	delete _control_allocation;
+	for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
+		delete _control_allocation[i];
+	}
+
 	delete _actuator_effectiveness;

 	perf_free(_loop_perf);
@@ -86,69 +89,85 @@ ControlAllocator::parameters_updated()
 {
 	// Allocation method & effectiveness source
 	// Do this first: in case a new method is loaded, it will be configured below
-	update_effectiveness_source();
-	update_allocation_method();
+	bool updated = update_effectiveness_source();
+	update_allocation_method(updated); // must be called after update_effectiveness_source()

-	if (_control_allocation == nullptr) {
+	if (_num_control_allocation == 0) {
 		return;
 	}

-	_control_allocation->updateParameters();
+	for (int i = 0; i < _num_control_allocation; ++i) {
+		_control_allocation[i]->updateParameters();
+	}
+
 	update_effectiveness_matrix_if_needed(true);
 }

 void
-ControlAllocator::update_allocation_method()
+ControlAllocator::update_allocation_method(bool force)
 {
-	AllocationMethod method = (AllocationMethod)_param_ca_method.get();
+	AllocationMethod configured_method = (AllocationMethod)_param_ca_method.get();

-	if (_allocation_method_id != method) {
+	if (!_actuator_effectiveness) {
+		PX4_ERR("_actuator_effectiveness null");
+		return;
+	}

-		// Save current state
-		matrix::Vector<float, NUM_ACTUATORS> actuator_sp;
+	if (_allocation_method_id != configured_method || force) {

-		if (_control_allocation != nullptr) {
-			actuator_sp = _control_allocation->getActuatorSetpoint();
+		matrix::Vector<float, NUM_ACTUATORS> actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES];
+
+		// Cleanup first
+		for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
+			// Save current state
+			if (_control_allocation[i] != nullptr) {
+				actuator_sp[i] = _control_allocation[i]->getActuatorSetpoint();
+			}
+
+			delete _control_allocation[i];
+			_control_allocation[i] = nullptr;
 		}

-		// try to instanciate new allocation method
-		ControlAllocation *tmp = nullptr;
+		_num_control_allocation = _actuator_effectiveness->numMatrices();

-		switch (method) {
-		case AllocationMethod::PSEUDO_INVERSE:
-			tmp = new ControlAllocationPseudoInverse();
-			break;
+		AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		_actuator_effectiveness->getDesiredAllocationMethod(desired_methods);

-		case AllocationMethod::SEQUENTIAL_DESATURATION:
-			tmp = new ControlAllocationSequentialDesaturation();
-			break;
+		for (int i = 0; i < _num_control_allocation; ++i) {
+			AllocationMethod method = configured_method;

-		default:
-			PX4_ERR("Unknown allocation method");
-			break;
+			if (configured_method == AllocationMethod::AUTO) {
+				method = desired_methods[i];
+			}
+
+			switch (method) {
+			case AllocationMethod::PSEUDO_INVERSE:
+				_control_allocation[i] = new ControlAllocationPseudoInverse();
+				break;
+
+			case AllocationMethod::SEQUENTIAL_DESATURATION:
+				_control_allocation[i] = new ControlAllocationSequentialDesaturation();
+				break;
+
+			default:
+				PX4_ERR("Unknown allocation method");
+				break;
+			}
+
+			if (_control_allocation[i] == nullptr) {
+				PX4_ERR("alloc failed");
+				_num_control_allocation = 0;
+
+			} else {
+				_control_allocation[i]->setActuatorSetpoint(actuator_sp[i]);
+			}
 		}

-		// Replace previous method with new one
-		if (tmp == nullptr) {
-			// It did not work, forget about it
-			PX4_ERR("Control allocation init failed");
-			_param_ca_method.set((int)_allocation_method_id);
-
-		} else {
-			// Swap allocation methods
-			delete _control_allocation;
-			_control_allocation = tmp;
-
-			// Save method id
-			_allocation_method_id = method;
-
-			// Configure new allocation method
-			_control_allocation->setActuatorSetpoint(actuator_sp);
-		}
+		_allocation_method_id = configured_method;
 	}
 }

-void
+bool
 ControlAllocator::update_effectiveness_source()
 {
 	EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
@@ -161,15 +180,15 @@ ControlAllocator::update_effectiveness_source()
 		switch (source) {
 		case EffectivenessSource::NONE:
 		case EffectivenessSource::MULTIROTOR:
-			tmp = new ActuatorEffectivenessMultirotor(this);
+			tmp = new ActuatorEffectivenessRotors(this);
 			break;

 		case EffectivenessSource::STANDARD_VTOL:
-			tmp = new ActuatorEffectivenessStandardVTOL();
+			tmp = new ActuatorEffectivenessStandardVTOL(this);
 			break;

 		case EffectivenessSource::TILTROTOR_VTOL:
-			tmp = new ActuatorEffectivenessTiltrotorVTOL();
+			tmp = new ActuatorEffectivenessTiltrotorVTOL(this);
 			break;

 		default:
@@ -191,7 +210,11 @@ ControlAllocator::update_effectiveness_source()
 			// Save source id
 			_effectiveness_source_id = source;
 		}
+
+		return true;
 	}
+
+	return false;
 }

 void
@@ -216,7 +239,7 @@ ControlAllocator::Run()
 		parameters_updated();
 	}

-	if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) {
+	if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) {
 		return;
 	}

@@ -281,18 +304,35 @@ ControlAllocator::Run()

 		update_effectiveness_matrix_if_needed();

-		// Set control setpoint vector
-		matrix::Vector<float, NUM_AXES> c;
-		c(0) = _torque_sp(0);
-		c(1) = _torque_sp(1);
-		c(2) = _torque_sp(2);
-		c(3) = _thrust_sp(0);
-		c(4) = _thrust_sp(1);
-		c(5) = _thrust_sp(2);
-		_control_allocation->setControlSetpoint(c);
+		// Set control setpoint vector(s)
+		matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		c[0](0) = _torque_sp(0);
+		c[0](1) = _torque_sp(1);
+		c[0](2) = _torque_sp(2);
+		c[0](3) = _thrust_sp(0);
+		c[0](4) = _thrust_sp(1);
+		c[0](5) = _thrust_sp(2);

-		// Do allocation
-		_control_allocation->allocate();
+		if (_num_control_allocation > 1) {
+			_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint);
+			_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint);
+			c[1](0) = vehicle_torque_setpoint.xyz[0];
+			c[1](1) = vehicle_torque_setpoint.xyz[1];
+			c[1](2) = vehicle_torque_setpoint.xyz[2];
+			c[1](3) = vehicle_thrust_setpoint.xyz[0];
+			c[1](4) = vehicle_thrust_setpoint.xyz[1];
+			c[1](5) = vehicle_thrust_setpoint.xyz[2];
+		}
+
+		for (int i = 0; i < _num_control_allocation; ++i) {
+
+			_control_allocation[i]->setControlSetpoint(c[i]);
+
+			// Do allocation
+			_control_allocation[i]->allocate();
+			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp);
+			_control_allocation[i]->clipActuatorSetpoint();
+		}

 		// Publish actuator setpoint and allocator status
 		publish_actuator_controls();
@@ -309,26 +349,61 @@ ControlAllocator::Run()
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
 {
-	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness;
+	ActuatorEffectiveness::Configuration config{};

-	if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness, force)) {
+	if (!force && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
+		return;
+	}

-		const matrix::Vector<float, NUM_ACTUATORS> &trim = _actuator_effectiveness->getActuatorTrim();
+	if (_actuator_effectiveness->getEffectivenessMatrix(config, force)) {
+		_last_effectiveness_update = hrt_absolute_time();

-		// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
-		matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
-		matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
+		memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes,
+		       sizeof(_control_allocation_selection_indexes));

-		for (size_t j = 0; j < NUM_ACTUATORS; j++) {
-			if (actuator_min(j) >= actuator_max(j)) {
-				for (size_t i = 0; i < NUM_AXES; i++) {
-					effectiveness(i, j) = 0.0f;
+		// Get the minimum and maximum depending on type and configuration
+		ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		int actuator_idx = 0;
+		int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
+
+		for (int actuator_type = 0; actuator_type < (int)ActuatorType::COUNT; ++actuator_type) {
+			_num_actuators[actuator_type] = config.num_actuators[actuator_type];
+
+			for (int actuator_type_idx = 0; actuator_type_idx < config.num_actuators[actuator_type]; ++actuator_type_idx) {
+				if (actuator_idx >= NUM_ACTUATORS) {
+					PX4_ERR("Too many actuators");
+					break;
 				}
+
+				int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+
+				if ((ActuatorType)actuator_type == ActuatorType::MOTORS) {
+					if (_param_r_rev.get() & (1u << actuator_type_idx)) {
+						minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
+
+					} else {
+						minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f;
+					}
+
+				} else {
+					minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
+				}
+
+				maximum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 1.f;
+				++actuator_idx_matrix[selected_matrix];
+				++actuator_idx;
 			}
 		}

-		// Assign control effectiveness matrix
-		_control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators());
+		for (int i = 0; i < _num_control_allocation; ++i) {
+			_control_allocation[i]->setActuatorMin(minimum[i]);
+			_control_allocation[i]->setActuatorMax(maximum[i]);
+
+			// Assign control effectiveness matrix
+			int total_num_actuators = config.num_actuators_matrix[i];
+			_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], total_num_actuators);
+		}
 	}
 }

@@ -338,8 +413,10 @@ ControlAllocator::publish_control_allocator_status()
 	control_allocator_status_s control_allocator_status{};
 	control_allocator_status.timestamp = hrt_absolute_time();

+	// TODO: handle multiple matrices & disabled motors (?)
+
 	// Allocated control
-	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation->getAllocatedControl();
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[0]->getAllocatedControl();
 	control_allocator_status.allocated_torque[0] = allocated_control(0);
 	control_allocator_status.allocated_torque[1] = allocated_control(1);
 	control_allocator_status.allocated_torque[2] = allocated_control(2);
@@ -348,7 +425,7 @@ ControlAllocator::publish_control_allocator_status()
 	control_allocator_status.allocated_thrust[2] = allocated_control(5);

 	// Unallocated control
-	matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation->getControlSetpoint() - allocated_control;
+	matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() - allocated_control;
 	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
 	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
 	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
@@ -363,11 +440,11 @@ ControlAllocator::publish_control_allocator_status()
 			unallocated_control(5)).norm_squared() < 1e-6f);

 	// Actuator saturation
-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation->getActuatorSetpoint();
-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation->getActuatorMin();
-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation->getActuatorMax();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[0]->getActuatorSetpoint();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation[0]->getActuatorMin();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation[0]->getActuatorMax();

-	for (size_t i = 0; i < NUM_ACTUATORS; i++) {
+	for (int i = 0; i < NUM_ACTUATORS; i++) {
 		if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {
 			control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER;

@@ -386,17 +463,57 @@ ControlAllocator::publish_actuator_controls()
 	actuator_motors.timestamp = hrt_absolute_time();
 	actuator_motors.timestamp_sample = _timestamp_sample;

-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation->getActuatorSetpoint();
-	matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
-				actuator_sp);
+	actuator_servos_s actuator_servos;
+	actuator_servos.timestamp = actuator_motors.timestamp;
+	actuator_servos.timestamp_sample = _timestamp_sample;

-	for (int i = 0; i < _control_allocation->numConfiguredActuators(); i++) {
-		actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
+	actuator_motors.reversible_flags = _param_r_rev.get();
+
+	int actuator_idx = 0;
+	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
+
+	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors();
+
+	// motors
+	int motors_idx;
+
+	for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
+		int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+		float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
+		actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
+
+		if (stopped_motors & (1u << motors_idx)) {
+			actuator_motors.control[motors_idx] = NAN;
+		}
+
+		++actuator_idx_matrix[selected_matrix];
+		++actuator_idx;
+	}
+
+	for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) {
+		actuator_motors.control[i] = NAN;
 	}

 	_actuator_motors_pub.publish(actuator_motors);

-	// TODO: servos
+	// servos
+	if (_num_actuators[1] > 0) {
+		int servos_idx;
+
+		for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) {
+			int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+			float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
+			actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
+			++actuator_idx_matrix[selected_matrix];
+			++actuator_idx;
+		}
+
+		for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) {
+			actuator_servos.control[i] = NAN;
+		}
+
+		_actuator_servos_pub.publish(actuator_servos);
+	}
 }

 int ControlAllocator::task_spawn(int argc, char *argv[])
@@ -439,6 +556,10 @@ int ControlAllocator::print_status()
 	case AllocationMethod::SEQUENTIAL_DESATURATION:
 		PX4_INFO("Method: Sequential desaturation");
 		break;
+
+	case AllocationMethod::AUTO:
+		PX4_INFO("Method: Auto");
+		break;
 	}

 	// Print current airframe
@@ -447,11 +568,20 @@ int ControlAllocator::print_status()
 	}

 	// Print current effectiveness matrix
-	if (_control_allocation != nullptr) {
-		const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness = _control_allocation->getEffectivenessMatrix();
-		PX4_INFO("Effectiveness.T =");
+	for (int i = 0; i < _num_control_allocation; ++i) {
+		const ActuatorEffectiveness::EffectivenessMatrix &effectiveness = _control_allocation[i]->getEffectivenessMatrix();
+
+		if (_num_control_allocation > 1) {
+			PX4_INFO("Instance: %i", i);
+		}
+
+		PX4_INFO("  Effectiveness.T =");
 		effectiveness.T().print();
-		PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators());
+		PX4_INFO("  minimum =");
+		_control_allocation[i]->getActuatorMin().T().print();
+		PX4_INFO("  maximum =");
+		_control_allocation[i]->getActuatorMax().T().print();
+		PX4_INFO("  Configured actuators: %i", _control_allocation[i]->numConfiguredActuators());
 	}

 	// Print perf