diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index a91b80cbbb..19c0034d1c 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -212,10 +212,6 @@ public: stopped_motors_mask |= _forwards_motors_mask; } - if (_upwards_motors_stopped_by_thrust) { - stopped_motors_mask |= _upwards_motors_mask; - } - // They can also be stopped in the ControlAllocator, due to // motor failure. The difference is that the stopped mask in the // allocator applies after the slew rate, and the mask here @@ -235,11 +231,6 @@ public: _forwards_motors_stopped_by_thrust = stopped; } - virtual void setUpwardsMotorsStoppedByThrust(bool stopped) - { - _upwards_motors_stopped_by_thrust = stopped; - } - protected: uint32_t _upwards_motors_mask{}; uint32_t _forwards_motors_mask{}; @@ -248,5 +239,4 @@ protected: uint32_t _stopped_motors_mask_due_to_flight_phase{0}; bool _forwards_motors_stopped_by_thrust{false}; - bool _upwards_motors_stopped_by_thrust{false}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 3ab965434b..416a3c0aef 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -399,21 +399,15 @@ ControlAllocator::Run() // As defined in VehicleThrustSetpoint.msg, a thrust of NaN means stopping the motor. // No sideways motors implemented for now... const bool stop_forwards_motors = std::isnan(_thrust_sp(0)); - const bool stop_upwards_motors = std::isnan(_thrust_sp(2)); // also only if it is actually about motors.... _actuator_effectiveness->setForwardsMotorsStoppedByThrust(stop_forwards_motors); - _actuator_effectiveness->setUpwardsMotorsStoppedByThrust(stop_upwards_motors); // Now that the intent of stopping motors is stored, transform // NaN to 0 to represent physical thrust again if (stop_forwards_motors) { _thrust_sp(0) = 0.f; } - - if (stop_upwards_motors) { - _thrust_sp(2) = 0.f; - } } if (do_update) {