diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 0ddd4988f3..26b31e102a 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -200,6 +200,8 @@ public: int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) {} + const ActuatorVector &getActuatorEffectivenessScale() { return _actuator_effectiveness_scale; } + /** * Get a bitmask of motors to be stopped */ @@ -222,4 +224,6 @@ public: protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; uint32_t _stopped_motors_mask{0}; + + ActuatorVector _actuator_effectiveness_scale; }; diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp index a6e2e2b254..0e19bc616c 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -143,7 +143,7 @@ public: * @return Control vector */ matrix::Vector getAllocatedControl() const - { return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); } + { return (_effectiveness * (_actuator_sp - _actuator_trim).emult(_actuator_effectiveness_scale)).emult(_control_allocation_scale); } /** * Get the control effectiveness matrix @@ -233,12 +233,15 @@ protected: matrix::Matrix _effectiveness; ///< Effectiveness matrix matrix::Vector _control_allocation_scale; ///< Scaling applied during allocation + matrix::Vector _actuator_sp_scale; ///< Scaling durectly applied to the actuator setpoint matrix::Vector _actuator_trim; ///< Neutral actuator values matrix::Vector _actuator_min; ///< Minimum actuator values matrix::Vector _actuator_max; ///< Maximum actuator values matrix::Vector _actuator_slew_rate_limit; ///< Slew rate limit matrix::Vector _prev_actuator_sp; ///< Previous actuator setpoint matrix::Vector _actuator_sp; ///< Actuator setpoint + matrix::Vector _actuator_effectiveness_scale; ///< Scaling applied after inilial + ///allocation (e.g.: when effectiveness changes due to airspeed or thrust) matrix::Vector _control_sp; ///< Control setpoint matrix::Vector _control_trim; ///< Control at trim actuator values int _num_actuators{0}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index e139d273e9..d44bef6bfd 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -442,6 +442,7 @@ ControlAllocator::Run() _actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + _control_allocation[i]->_actuator_effectiveness_scale = _actuator_effectiveness->getActuatorEffectivenessScale(); if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index ecd7b51f4c..2fc9407203 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -187,10 +187,17 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, } } -void ActuatorEffectivenessControlSurfaces::applyScale(float scale, int first_actuator_idx, - ActuatorVector &actuator_sp) const +void ActuatorEffectivenessControlSurfaces::applyEffectivenessScale(float effectiveness_scale, int first_actuator_idx, + ActuatorVector &actuator_sp, ActuatorVector &applied_effectiveness_scale) const { + if (!PX4_ISFINITE(effectiveness_scale)) { + effectiveness_scale = 1.f; + } + + const float actuator_scale = 1.f / fmaxf(effectiveness_scale, FLT_EPSILON); + for (int i = 0; i < _count; ++i) { - actuator_sp(i + first_actuator_idx) *= scale; + actuator_sp(i + first_actuator_idx) *= actuator_scale; + applied_effectiveness_scale(i + first_actuator_idx) = effectiveness_scale; } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index b62f09082c..0573c9e8aa 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -91,7 +91,7 @@ public: void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp); void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp); - void applyScale(float scale, int first_actuator_idx, ActuatorVector &actuator_sp) const; + void applyEffectivenessScale(float scale, int first_actuator_idx, ActuatorVector &actuator_sp, ActuatorVector &applied_effectiveness_scale) const; private: void updateParams() override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp index 5a68352cbb..d1e4869453 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp @@ -83,8 +83,8 @@ void ActuatorEffectivenessThrustVectoring::updateSetpoint(const matrix::Vector