From f44713e8a35d6c2770a295f2be7fc3f9ee545a41 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 31 Oct 2022 11:09:00 +0100 Subject: [PATCH] ControlAllocator: enable custom saturation logic to override default one Custom saturation logic currently implemented for Tiltrotor VTOL and Helicopter. Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectiveness.hpp | 6 +-- .../ActuatorEffectivenessHelicopter.cpp | 15 +----- .../ActuatorEffectivenessHelicopter.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 36 +++++++++++++ .../ActuatorEffectivenessTiltrotorVTOL.hpp | 10 ++++ .../control_allocator/ControlAllocator.cpp | 50 ++++++++++--------- 6 files changed, 78 insertions(+), 41 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 8a1f0df1f4..344c468a16 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. - * Should return false if not filled in and the effectivenes matrix should be used instead + * Fill in the allocated and 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 bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; } + virtual void getAllocatedAndUnallocatedControl(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 8cf7a9cb5d..a0ca1808d8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -217,48 +217,37 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit } } -bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const +void ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(int matrix_index, + control_allocator_status_s &status) { - status.torque_setpoint_achieved = true; - status.thrust_setpoint_achieved = true; // Note: the values '-1', '1' and '0' are just to indicate a negative, // positive or no saturation to the rate controller. The actual magnitude is not used. if (_saturation_flags.roll_pos) { status.unallocated_torque[0] = 1.f; - status.torque_setpoint_achieved = false; } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; - status.torque_setpoint_achieved = false; } if (_saturation_flags.pitch_pos) { status.unallocated_torque[1] = 1.f; - status.torque_setpoint_achieved = false; } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; - status.torque_setpoint_achieved = false; } if (_saturation_flags.yaw_pos) { status.unallocated_torque[2] = 1.f; - status.torque_setpoint_achieved = false; } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; - status.torque_setpoint_achieved = false; } if (_saturation_flags.thrust_pos) { status.unallocated_thrust[2] = 1.f; - status.thrust_setpoint_achieved = false; } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; - status.thrust_setpoint_achieved = false; } - - return true; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 64bf52138b..83d28d2330 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; - bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override; + void getAllocatedAndUnallocatedControl(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 6f5301a7ef..aee4f78644 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 1.f) { + _yaw_tilt_saturation_flags.tilt_yaw_pos = true; + } + } } void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) @@ -170,3 +185,24 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh break; } } + +void ActuatorEffectivenessTiltrotorVTOL::getAllocatedAndUnallocatedControl(int matrix_index, + control_allocator_status_s &status) +{ + // only handle matrix 0 (motors and tilts) + if (matrix_index == 1) { + return; + } + + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_yaw_tilt_saturation_flags.tilt_yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index d7d136727c..7ad8477e02 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -81,6 +81,9 @@ public: const char *name() const override { return "VTOL Tiltrotor"; } uint32_t getStoppedMotors() const override { return _stopped_motors; } + + void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + protected: bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; @@ -97,4 +100,11 @@ protected: uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)}; uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index fcd4209cfa..dee3cb8e6a 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -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 &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 &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() - - 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 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 &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();