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

ControlAllocator: enable custom saturation logic to override default one

Browse Source 
Custom saturation logic currently implemented for Tiltrotor VTOL and Helicopter.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-10-31 11:09:00 +01:00
committed by Beat Küng
parent 2e20fb7f97
commit f44713e8a3
6 changed files with 78 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+26 -24
View File
@@ -589,33 +589,35 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
// TODO: disabled motors (?)
if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
// Allocated control
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->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);
control_allocator_status.allocated_thrust[0] = allocated_control(3);
control_allocator_status.allocated_thrust[1] = allocated_control(4);
control_allocator_status.allocated_thrust[2] = allocated_control(5);
// Allocated control
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->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);
control_allocator_status.allocated_thrust[0] = allocated_control(3);
control_allocator_status.allocated_thrust[1] = allocated_control(4);
control_allocator_status.allocated_thrust[2] = allocated_control(5);
// Unallocated control
const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->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);
control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
// Unallocated control
const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->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);
control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
// Allocation success flags
control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
unallocated_control(2)).norm_squared() < 1e-6f);
control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
unallocated_control(5)).norm_squared() < 1e-6f);
}
// override control_allocator_status in customized saturation logic for certain effectiveness types
_actuator_effectiveness->getAllocatedAndUnallocatedControl(matrix_index, control_allocator_status);
// Allocation success flags
control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
control_allocator_status.unallocated_torque[1],
control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
control_allocator_status.unallocated_thrust[1],
control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
// Actuator saturation
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();