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

CA: log torque setpoints

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-06-04 11:56:59 +02:00
parent 9bdc898ceb
commit 2ef7c45d97
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/ControlAllocatorStatus.msg
View File

@ -1,5 +1,8 @@
uint64 timestamp # time since system start (microseconds)
float32[3] torque_setpoint # Current torque setpoint (normalized) for specific allocation matrix.
float32[3] thrust_setpoint # Current thrust setpoint (normalized) for specific allocation matrix.
bool torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_torque = torque_setpoint - allocated_torque

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

@ -705,6 +705,13 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
control_allocator_status.torque_setpoint[0] = _control_allocation[matrix_index]->getControlSetpoint()(0);
control_allocator_status.torque_setpoint[1] = _control_allocation[matrix_index]->getControlSetpoint()(1);
control_allocator_status.torque_setpoint[2] = _control_allocation[matrix_index]->getControlSetpoint()(2);
control_allocator_status.thrust_setpoint[0] = _control_allocation[matrix_index]->getControlSetpoint()(3);
control_allocator_status.thrust_setpoint[1] = _control_allocation[matrix_index]->getControlSetpoint()(4);
control_allocator_status.thrust_setpoint[2] = _control_allocation[matrix_index]->getControlSetpoint()(5);
// override control_allocator_status in customized saturation logic for certain effectiveness types
_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);