From 2ef7c45d97bf0fd9721e618edf44f402018d2ee9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 4 Jun 2024 11:56:59 +0200 Subject: [PATCH] CA: log torque setpoints Signed-off-by: Silvan Fuhrer --- msg/ControlAllocatorStatus.msg | 3 +++ src/modules/control_allocator/ControlAllocator.cpp | 7 +++++++ 2 files changed, 10 insertions(+) diff --git a/msg/ControlAllocatorStatus.msg b/msg/ControlAllocatorStatus.msg index 2d7b088322..b3dc530f22 100644 --- a/msg/ControlAllocatorStatus.msg +++ b/msg/ControlAllocatorStatus.msg @@ -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 diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 80f981a0c6..1c8313962c 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -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);