diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index b36da8230f..89a7e69c60 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -83,21 +83,3 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const return total_count; } - -void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) -{ - 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) { - _stopped_motors_mask |= motor_mask; - - } else { - _stopped_motors_mask &= ~motor_mask; - } - } - } -} diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index ffc2315b72..0bc2d0cf58 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -209,14 +209,6 @@ 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 - * - * @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 - */ - virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); - protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; uint32_t _stopped_motors_mask{0}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index 6e5ccbe44a..8b7dd621d2 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -58,9 +58,3 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration return (motors_added_successfully && torque_added_successfully); } - -void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) -{ - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); -} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index e2f5d4dfa6..0abbb26546 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -45,9 +45,6 @@ public: bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, - const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; - const char *name() const override { return "Custom"; } protected: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index e91783cf80..bbbe94442d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -61,12 +61,6 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat return (rotors_added_successfully && surfaces_added_successfully); } -void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) -{ - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); -} - void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 0633cbc875..9208226280 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -51,9 +51,6 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, - const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; - private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 45256b87cc..08ca0fae70 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -48,9 +48,3 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{}); return true; } - -void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) -{ - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); -} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 82b07733bf..27478746ec 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -43,9 +43,6 @@ public: bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, - const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; - const char *name() const override { return "Rover (Ackermann)"; } private: uint32_t _motors_mask{}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 0b7d3934c1..063dcc00aa 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -83,14 +83,6 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, } } -void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) -{ - if (matrix_index == 0) { - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); - } -} - void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) { if (_flight_phase == flight_phase) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 4879c6b022..cebcdf97b6 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -75,9 +75,6 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, - const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; - void setFlightPhase(const FlightPhase &flight_phase) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 05fcaf675e..e908ab06b8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -88,14 +88,6 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d } } -void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) -{ - if (matrix_index == 0) { - stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); - } -} - void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase) { if (_flight_phase == flight_phase) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index c7a588954f..7ee2c82356 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -72,10 +72,6 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, - const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; - - void setFlightPhase(const FlightPhase &flight_phase) override; const char *name() const override { return "VTOL Tailsitter"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 3042ab825b..fb3eb541cb 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -205,10 +205,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) -{ - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); -} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 78e52c34aa..9c337c2c70 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -54,9 +54,6 @@ public: normalize[0] = true; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, - const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; - const char *name() const override { return "UUV"; } protected: