diff --git a/msg/ControlAllocatorStatus.msg b/msg/ControlAllocatorStatus.msg index 3fbbe844ef..2d7b088322 100644 --- a/msg/ControlAllocatorStatus.msg +++ b/msg/ControlAllocatorStatus.msg @@ -1,12 +1,10 @@ uint64 timestamp # time since system start (microseconds) 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] allocated_torque # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved. float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved. # Computed as: unallocated_torque = torque_setpoint - allocated_torque bool thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. -float32[3] allocated_thrust # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved. float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved. # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 344c468a16..952179c81f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -199,10 +199,10 @@ public: virtual uint32_t getStoppedMotors() const { return 0; } /** - * Fill in the allocated and unallocated torque and thrust, customized by effectiveness type. + * Fill in the unallocated torque and thrust, customized by effectiveness type. * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead. */ - virtual void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index a0ca1808d8..f5a7aaedbb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -217,8 +217,7 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit } } -void ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(int matrix_index, - control_allocator_status_s &status) +void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { // Note: the values '-1', '1' and '0' are just to indicate a negative, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 83d28d2330..3cd64b2f36 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -79,7 +79,7 @@ public: ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) override; - void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: float throttleSpoolupProgress(); bool mainMotorEnaged(); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index aee4f78644..bd9e1db0fc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -186,8 +186,7 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh } } -void ActuatorEffectivenessTiltrotorVTOL::getAllocatedAndUnallocatedControl(int matrix_index, - control_allocator_status_s &status) +void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { // only handle matrix 0 (motors and tilts) if (matrix_index == 1) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 7ad8477e02..5686d2ebda 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -82,7 +82,7 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } - void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; protected: bool _collective_tilt_updated{true}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index dee3cb8e6a..c487b4bc4b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -591,12 +591,6 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) // Allocated control const matrix::Vector &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 unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - @@ -609,7 +603,7 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) control_allocator_status.unallocated_thrust[2] = unallocated_control(5); // override control_allocator_status in customized saturation logic for certain effectiveness types - _actuator_effectiveness->getAllocatedAndUnallocatedControl(matrix_index, control_allocator_status); + _actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status); // Allocation success flags control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],