From 0e66b0876b003bf57f4aec1e6825aca23b041b2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:47 +0200 Subject: [PATCH] control_allocator: change SequentialDesaturation to existing MC mixer And limit the operations to the number of configured outputs. Only using the number of configured actuators reduces CPU load by ~2% on F7 @1khz. --- .../ActuatorEffectiveness.hpp | 5 + .../ActuatorEffectivenessMultirotor.cpp | 14 +- .../ActuatorEffectivenessMultirotor.hpp | 6 +- .../ActuatorEffectivenessStandardVTOL.hpp | 1 + .../ActuatorEffectivenessTiltrotorVTOL.hpp | 1 + .../ControlAllocation/ControlAllocation.cpp | 31 ++-- .../ControlAllocation/ControlAllocation.hpp | 11 +- .../ControlAllocationPseudoInverse.cpp | 6 +- .../ControlAllocationPseudoInverse.hpp | 2 +- ...ontrolAllocationSequentialDesaturation.cpp | 170 +++++++++++++++--- ...ontrolAllocationSequentialDesaturation.hpp | 89 ++++++--- .../control_allocator/ControlAllocator.cpp | 5 + 12 files changed, 259 insertions(+), 82 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index f0ac4bcb92..bd6ea7af11 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -99,6 +99,11 @@ public: return _flight_phase; } + /** + * Get the number of actuators + */ + virtual int numActuators() const = 0; + protected: matrix::Vector _trim; ///< Actuator trim FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index a30ac2fde6..e2e7d2e883 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -132,17 +132,19 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &effectiveness) { + int num_actuators = 0; + for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { // Get rotor axis matrix::Vector3f axis( @@ -173,6 +175,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom float ct = geometry.rotors[i].thrust_coef; float km = geometry.rotors[i].moment_ratio; + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + // Compute thrust generated by this rotor matrix::Vector3f thrust = ct * axis; @@ -184,5 +190,9 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom effectiveness(j, i) = moment(j); effectiveness(j + 3, i) = thrust(j); } + + num_actuators = i + 1; } + + return num_actuators; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index c5be27fc91..1bb4c9efc2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -70,16 +70,18 @@ public: RotorGeometry rotors[NUM_ROTORS_MAX]; } MultirotorGeometry; - static void computeEffectivenessMatrix(const MultirotorGeometry &geometry, - matrix::Matrix &effectiveness); + static int computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness); bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + int numActuators() const override { return _num_actuators; } private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ bool _updated{true}; + int _num_actuators{0}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_mc_r0_px, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 01cff0a764..135dc976df 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -58,6 +58,7 @@ public: */ void setFlightPhase(const FlightPhase &flight_phase) override; + int numActuators() const override { return 7; } protected: bool _updated{true}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index d2f0633341..3c266b833c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -58,6 +58,7 @@ public: */ void setFlightPhase(const FlightPhase &flight_phase) override; + int numActuators() const override { return 10; } protected: bool _updated{true}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 4242f99cb5..f32522fdad 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -68,11 +68,13 @@ ControlAllocation::getAllocatedControl() const void ControlAllocation::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim) + const matrix::Vector &actuator_trim, int num_actuators) { _effectiveness = effectiveness; - _actuator_trim = clipActuatorSetpoint(actuator_trim); + _actuator_trim = actuator_trim; + clipActuatorSetpoint(_actuator_trim); _control_trim = _effectiveness * _actuator_trim; + _num_actuators = num_actuators; } const matrix::Matrix & @@ -115,34 +117,27 @@ ControlAllocation::setActuatorSetpoint( _actuator_sp = actuator_sp; // Clip - _actuator_sp = clipActuatorSetpoint(_actuator_sp); + clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; } -matrix::Vector -ControlAllocation::clipActuatorSetpoint(const matrix::Vector &actuator) const +void +ControlAllocation::clipActuatorSetpoint(matrix::Vector &actuator) const { - matrix::Vector actuator_clipped; - - for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { + for (int i = 0; i < _num_actuators; i++) { if (_actuator_max(i) < _actuator_min(i)) { - actuator_clipped(i) = _actuator_trim(i); + actuator(i) = _actuator_trim(i); - } else if (actuator_clipped(i) < _actuator_min(i)) { - actuator_clipped(i) = _actuator_min(i); + } else if (actuator(i) < _actuator_min(i)) { + actuator(i) = _actuator_min(i); - } else if (actuator_clipped(i) > _actuator_max(i)) { - actuator_clipped(i) = _actuator_max(i); - - } else { - actuator_clipped(i) = actuator(i); + } else if (actuator(i) > _actuator_max(i)) { + actuator(i) = _actuator_max(i); } } - - return actuator_clipped; } matrix::Vector diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 1423520f72..6cc0529b04 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -105,7 +105,7 @@ public: * @param B Effectiveness matrix */ virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim); + const matrix::Vector &actuator_trim, int num_actuators); /** * Get the allocated actuator vector @@ -187,10 +187,8 @@ public: * The output is in the range [min; max] * * @param actuator Actuator vector to clip - * - * @return Clipped actuator setpoint */ - matrix::Vector clipActuatorSetpoint(const matrix::Vector &actuator) const; + void clipActuatorSetpoint(matrix::Vector &actuator) const; /** * Normalize the actuator setpoint between minimum and maximum values. @@ -204,6 +202,10 @@ public: matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) const; + virtual void updateParameters() {} + + int numConfiguredActuators() const { return _num_actuators; } + protected: matrix::Matrix _effectiveness; //< Effectiveness matrix matrix::Vector _actuator_trim; //< Neutral actuator values @@ -213,4 +215,5 @@ protected: matrix::Vector _control_sp; //< Control setpoint matrix::Vector _control_allocated; //< Allocated control matrix::Vector _control_trim; //< Control at trim actuator values + int _num_actuators{0}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index c4a57ffb9c..b613e77c73 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -44,9 +44,9 @@ void ControlAllocationPseudoInverse::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim) + const matrix::Vector &actuator_trim, int num_actuators) { - ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim); + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators); _mix_update_needed = true; } @@ -69,7 +69,7 @@ ControlAllocationPseudoInverse::allocate() _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); // Clip - _actuator_sp = clipActuatorSetpoint(_actuator_sp); + clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index c39c6c19f2..148b5af6cd 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -55,7 +55,7 @@ public: virtual void allocate() override; virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim) override; + const matrix::Vector &actuator_trim, int num_actuators) override; protected: matrix::Matrix _mix; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 6af2bd472c..a497fd8c68 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -35,28 +35,36 @@ * @file ControlAllocationSequentialDesaturation.cpp * * @author Roman Bapst + * @author Beat Küng */ #include "ControlAllocationSequentialDesaturation.hpp" - void ControlAllocationSequentialDesaturation::allocate() { //Compute new gains if needed updatePseudoInverse(); - // Allocate - _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); + switch (_param_mc_airmode.get()) { + case 1: + mixAirmodeRP(); + break; - // go through control axes from lowest to highest priority and unsaturate the actuators - for (unsigned i = 0; i < NUM_AXES; i++) { - desaturateActuators(_actuator_sp, _axis_prio_increasing[i]); + case 2: + mixAirmodeRPY(); + break; + + default: + mixAirmodeDisabled(); + break; } + // TODO: thrust model (THR_MDL_FAC) + // Clip - _actuator_sp = clipActuatorSetpoint(_actuator_sp); + clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; @@ -64,31 +72,24 @@ ControlAllocationSequentialDesaturation::allocate() void ControlAllocationSequentialDesaturation::desaturateActuators( ActuatorVector &actuator_sp, - const ControlAxis &axis) + const ActuatorVector &desaturation_vector, bool increase_only) { - ActuatorVector desaturation_vector = getDesaturationVector(axis); - float gain = computeDesaturationGain(desaturation_vector, actuator_sp); - actuator_sp = actuator_sp + gain * desaturation_vector; - - gain = computeDesaturationGain(desaturation_vector, actuator_sp); - - actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector; -} - -ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector( - const ControlAxis &axis) -{ - ActuatorVector ret; - - for (unsigned i = 0; i < NUM_ACTUATORS; i++) { - ret(i) = _mix(i, axis); + if (increase_only && gain < 0.f) { + return; } - return ret; -} + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } + gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } +} float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp) @@ -96,7 +97,7 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act float k_min = 0.f; float k_max = 0.f; - for (unsigned i = 0; i < NUM_ACTUATORS; i++) { + for (int i = 0; i < _num_actuators; i++) { // Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway if (fabsf(desaturation_vector(i)) < FLT_EPSILON) { continue; @@ -122,3 +123,118 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act // Reduce the saturation as much as possible return k_min + k_max; } + +void +ControlAllocationSequentialDesaturation::mixAirmodeRP() +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRPY() +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + ActuatorVector thrust_z; + ActuatorVector yaw; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + yaw(i) = _mix(i, ControlAxis::YAW); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + desaturateActuators(_actuator_sp, yaw); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeDisabled() +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + ActuatorVector thrust_z; + ActuatorVector roll; + ActuatorVector pitch; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + } + + // only reduce thrust + desaturateActuators(_actuator_sp, thrust_z, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + desaturateActuators(_actuator_sp, roll); + desaturateActuators(_actuator_sp, pitch); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixYaw() +{ + // Add yaw to outputs + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + ActuatorVector max_prev = _actuator_max; + _actuator_max += (_actuator_max - _actuator_min) * 0.15f; + desaturateActuators(_actuator_sp, yaw); + _actuator_max = max_prev; + + // reduce thrust only + desaturateActuators(_actuator_sp, thrust_z, true); +} + +void +ControlAllocationSequentialDesaturation::updateParameters() +{ + updateParams(); +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp index 6765a25322..53c422cd39 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp @@ -45,45 +45,84 @@ #include "ControlAllocationPseudoInverse.hpp" -class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse +#include + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams { public: - ControlAllocationSequentialDesaturation() = default; + ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} virtual ~ControlAllocationSequentialDesaturation() = default; void allocate() override; + void updateParameters() override; private: /** - * List of control axis used for desaturating the actuator vector. The desaturation logic will sequentially - * go through this list and if needed apply corrections to the demand of the corresponding axis in order to desaturate - * the actuator vector. + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param actuator_sp Actuator setpoint, vector that is modified + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector */ - ControlAxis _axis_prio_increasing [NUM_AXES] = {ControlAxis::YAW, ControlAxis::THRUST_Y, ControlAxis::THRUST_X, ControlAxis::THRUST_Z, ControlAxis::PITCH, ControlAxis::ROLL}; + void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + bool increase_only = false); /** - * Desaturate actuator setpoint vector. + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. * - * @return Desaturated actuator setpoint vector. - */ - void desaturateActuators(ActuatorVector &actuator_sp, const ControlAxis &axis); - - /** - * Get desaturation vector. - * - * @param axis Control axis - * @return ActuatorVector Column of the pseudo-inverse matrix corresponding to the given control axis. - */ - ActuatorVector getDesaturationVector(const ControlAxis &axis); - - /** - * Compute desaturation gain. - * - * @param desaturation_vector Column of the pseudo-inverse matrix corresponding to a given control axis. - * @param Actuator setpoint vector. - * @return Gain which eliminates the saturation of the highest saturated actuator. + * @return desaturation gain */ float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + void mixAirmodeRP(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + void mixAirmodeRPY(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mixYaw() for the exact yaw behavior. + */ + void mixAirmodeDisabled(); + + /** + * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + */ + void mixYaw(); + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< air-mode + ); }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index eaa2647069..b0bf782302 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -249,6 +249,10 @@ ControlAllocator::Run() parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); + if (_control_allocation) { + _control_allocation->updateParameters(); + } + updateParams(); parameters_updated(); } @@ -520,6 +524,7 @@ int ControlAllocator::print_status() const matrix::Matrix &effectiveness = _control_allocation->getEffectivenessMatrix(); PX4_INFO("Effectiveness.T ="); effectiveness.T().print(); + PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators()); } // Print perf