From 3c739352eb9ca1f68e16821649bddddcf8584776 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 7 Apr 2025 16:52:09 +0200 Subject: [PATCH] Allocation: expose threshold to decare motor stopped in new param CA_STOP_MOT_THLD Instead of outputting a command to minimally rotate a motor, it is desirable to competely stop it if the commanded setpoints is below some value. E.g. a tiltable motor on a tiltrotor VTOL should be stopped if the commanded thrust is 0 in fixed-wing mode, but not stopped when in hover (thus PWM_MIN has to be configured to spin the motor). This commit replaces the previous hard-coded limit of 2% motor setpoint with the new parameter CA_STOP_MOT_THLD. Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectiveness.cpp | 7 ++++--- .../ActuatorEffectiveness.hpp | 8 +++++--- src/modules/control_allocator/ControlAllocator.cpp | 2 +- src/modules/control_allocator/ControlAllocator.hpp | 3 ++- .../ActuatorEffectivenessCustom.cpp | 4 ++-- .../ActuatorEffectivenessCustom.hpp | 2 +- .../ActuatorEffectivenessFixedWing.cpp | 4 ++-- .../ActuatorEffectivenessFixedWing.hpp | 2 +- .../ActuatorEffectivenessHelicopter.cpp | 2 +- .../ActuatorEffectivenessHelicopter.hpp | 2 +- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 2 +- .../ActuatorEffectivenessHelicopterCoaxial.hpp | 2 +- .../ActuatorEffectivenessHelicopterTest.cpp | 3 ++- .../ActuatorEffectivenessMCTilt.cpp | 2 +- .../ActuatorEffectivenessMCTilt.hpp | 2 +- .../ActuatorEffectivenessRoverAckermann.cpp | 4 ++-- .../ActuatorEffectivenessRoverAckermann.hpp | 2 +- .../ActuatorEffectivenessStandardVTOL.cpp | 4 ++-- .../ActuatorEffectivenessStandardVTOL.hpp | 2 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 4 ++-- .../ActuatorEffectivenessTailsitterVTOL.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 4 ++-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 2 +- .../ActuatorEffectivenessUUV.cpp | 4 ++-- .../ActuatorEffectivenessUUV.hpp | 2 +- src/modules/control_allocator/module.yaml | 14 ++++++++++++++ 26 files changed, 55 insertions(+), 36 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index b36da8230f..a9d0ef814f 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -84,15 +84,16 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const return total_count; } -void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) +void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp, + float threshold) { for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) { const uint32_t motor_mask = (1u << actuator_idx); if (stoppable_motors_mask & motor_mask) { - // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy) - if (fabsf(actuator_sp(actuator_idx)) < .02f) { + // Stop motor if its setpoint is below threshold. + if (fabsf(actuator_sp(actuator_idx)) < threshold) { _stopped_motors_mask |= motor_mask; } else { diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 0ddd4988f3..c880be8227 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -198,7 +198,7 @@ public: */ virtual void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) {} + const matrix::Vector &actuator_max, float stop_threshold) {} /** * Get a bitmask of motors to be stopped @@ -212,12 +212,14 @@ public: virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} /** - * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero + * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is below threshold * * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand * @param actuator_sp outcome of the allocation to determine if the motor should be stopped + * @param threshold actuator setpoint threshold below which the motor is stopped */ - virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); + virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp, + float threshold); protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 99e280dd71..8714d9dde4 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -445,7 +445,7 @@ ControlAllocator::Run() _control_allocation[i]->allocate(); _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]->getActuatorMin(), _control_allocation[i]->getActuatorMax(), _param_ca_stop_mot_thld.get()); if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 1f91f7d17d..e303d31c1a 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -216,7 +216,8 @@ private: (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, (ParamInt) _param_ca_failure_mode, - (ParamInt) _param_r_rev + (ParamInt) _param_r_rev, + (ParamFloat) _param_ca_stop_mot_thld ) }; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index ead2043682..0d153b7abc 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -61,7 +61,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp, stop_threshold); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index b7906669f2..957d468541 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -47,7 +47,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; const char *name() const override { return "Custom"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 0a4516ba56..31b8d3e31a 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -63,9 +63,9 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp, stop_threshold); } void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index, diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index f0b095709e..6086c1f12e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -53,7 +53,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; private: ActuatorEffectivenessRotors _rotors; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 967706a6d8..3a25357537 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -145,7 +145,7 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 78df3abec1..584702463b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -83,7 +83,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 40e956df99..19e226b281 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -103,7 +103,7 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index e97c31c0da..d64b0b6e3e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -71,7 +71,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp index 29ca1f1117..0a91d021e0 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp @@ -64,9 +64,10 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve) actuator_min.setAll(0.f); ActuatorEffectiveness::ActuatorVector actuator_max{}; actuator_max.setAll(1.f); + const float stop_threshold = 0.f; // do not stop motors control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.1f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max, 0.f); EXPECT_FLOAT_EQ(actuator_sp(0), 0.f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.f; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index a8effa8cb3..99fc56b620 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -78,7 +78,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { actuator_sp += _tilt_offsets; // TODO: dynamic matrix update diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 0b12482781..0e26e458b0 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -57,7 +57,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; const char *name() const override { return "MC Tilt"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index e9eda4c538..0b809fa734 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -51,7 +51,7 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp, stop_threshold); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index f6108b4baf..fc9b60ba5a 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -45,7 +45,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; const char *name() const override { return "Rover (Ackermann)"; } private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index f15624dd67..7bc809f422 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -85,10 +85,10 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { if (matrix_index == 0) { - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp, stop_threshold); } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 9f945a6cd8..b9531d970d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -77,7 +77,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 559101c106..b0fb40688e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -90,10 +90,10 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { if (matrix_index == 0) { - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp, stop_threshold); } } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 708f104faa..85d0b75b06 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -74,7 +74,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 93ac6783d6..7aa06abecf 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -126,7 +126,7 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { // apply tilt if (matrix_index == 0) { @@ -208,7 +208,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; const char *name() const override { return "VTOL Tiltrotor"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp index e4c861a9d0..b8d08192c9 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp @@ -57,7 +57,7 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + const matrix::Vector &actuator_max, float stop_threshold) { - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp, stop_threshold); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 1b89da8d9b..be72aaf46a 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -56,7 +56,7 @@ public: void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + const matrix::Vector &actuator_max, float stop_threshold) override; const char *name() const override { return "UUV"; } diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 82d3fb2e12..7db8e69002 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -592,6 +592,20 @@ parameters: 1: Remove first failed motor from effectiveness default: 0 + CA_STOP_MOT_THLD: + description: + short: Threshold on absolute thrust setpoint to stop stoppable motors. + long: | + Stop motor (send NAN) if absolute actuator setpoint is below this threshold on + a stoppable motor (e.g. fixed-wing motors). + Set to 0 to not stop any motor. + type: float + decimal: 2 + increment: 0.01 + min: 0 + max: 1 + default: 0.02 + # Mixer mixer: actuator_types: