From debfdefc418159e1cc13a4c6e598679fd7a18ff1 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 24 Aug 2023 13:58:58 +0200 Subject: [PATCH] Control Allocation: fixes in yaw saturation detection for vehicles with tilt-for-yaw (#21994) * ActuatorEffectiveness: base yaw saturation on tilt actuator limits (VTOL Tiltrotor) * ActuatorEffectiveness: add custom yaw saturation for MC Tilt --------- Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectivenessMCTilt.cpp | 51 +++++++++++++++++-- .../ActuatorEffectivenessMCTilt.hpp | 12 ++++- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 42 +++++++++------ .../ActuatorEffectivenessTilts.hpp | 2 + 4 files changed, 88 insertions(+), 19 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 41005d38a717..a8effa8cb315 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,7 +55,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); // Tilts - int first_tilt_idx = configuration.num_actuators_matrix[0]; + _first_tilt_idx = configuration.num_actuators_matrix[0]; _tilts.updateTorqueSign(_mc_rotors.geometry()); const bool tilts_added_successfully = _tilts.addActuators(configuration); @@ -69,7 +69,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration if (delta_angle > FLT_EPSILON) { float trim = -1.f - 2.f * _tilts.config(i).min_angle / delta_angle; - _tilt_offsets(first_tilt_idx + i) = trim; + _tilt_offsets(_first_tilt_idx + i) = trim; } } @@ -82,4 +82,49 @@ void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } + } + + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; +} + +void ActuatorEffectivenessMCTilt::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // 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/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 15c12957ed17..848c8b8853d3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -61,8 +61,18 @@ class ActuatorEffectivenessMCTilt : public ModuleParams, public ActuatorEffectiv const char *name() const override { return "MC Tilt"; } + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + protected: ActuatorVector _tilt_offsets; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessTilts _tilts; + int _first_tilt_idx{0}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 970c2dbf6751..f24d62a07d25 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -148,6 +148,9 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } } + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; + // in FW directly use throttle sp if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { for (int i = 0; i < _first_tilt_idx; ++i) { @@ -173,21 +200,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 1.f) { - _yaw_tilt_saturation_flags.tilt_yaw_pos = true; - } - } } void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index a6f1733c1058..d885a091552b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -79,6 +79,8 @@ class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffective bool hasYawControl() const; + float getYawTorqueOfTilt(int tilt_index) const { return _torque[tilt_index](2); } + private: void updateParams() override;