Allocator: refactor stopped motor mask

The stopped motor mask is currently a shared piece of state that is used
for two purposes:
 - stopping motors due to the flight phase
 - stopping motors due to low thrust (removed in previous commit)

To handle the shared state more neatly we store the two conditions that
stop motors separately:
 - For stopping motors because they are not used in the current flight
   phase, the bitmask _stopped_motors_mask_due_to_flight_phase is used.
   By default it is 0, and individual ActuatorEffectiveness* classes may
   update it in setFlightPhase
 - For stopping motors due to NaN thrust setpoint, we have the flags
   _{forwards,upwards}_motors_stopped_by_thrust with their setters
 - in getStoppedMotors, the two are combined to form the overall rotors
   stopped mask

By separating the reasons like this we remove the need of carrying over
state in the different setFlightPhase methods (&=, |=).

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -201,7 +201,28 @@ public:
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
-	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
+	uint32_t getStoppedMotors() const
+	{
+		// Motors can be stopped (here) for two reasons:
+		//  a) they are turned off because they are generally not used in the given flight phase.
+		uint32_t stopped_motors_mask = _stopped_motors_mask_due_to_flight_phase;
+
+		//  b) they are stopped because the thrust setpoint in a given direction is NaN
+		if (_forwards_motors_stopped_by_thrust) {
+			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
+		// applies before.
+
+		return stopped_motors_mask;
+	}
 
 	/**
 	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
@@ -209,7 +230,23 @@ public:
 	 */
 	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
 
+	virtual void setForwardsMotorsStoppedByThrust(bool stopped)
+	{
+		_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{};
+
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
-	uint32_t _stopped_motors_mask{0};
+	uint32_t _stopped_motors_mask_due_to_flight_phase{0};
+
+	bool _forwards_motors_stopped_by_thrust{false};
+	bool _upwards_motors_stopped_by_thrust{false};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -395,7 +395,25 @@ ControlAllocator::Run()
 
 	if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
 		_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
-		// TODO: stop the motors on axes that have NAN
+
+		// 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) {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -91,16 +91,15 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors_mask |= _upwards_motors_mask;
+		_stopped_motors_mask_due_to_flight_phase = _upwards_motors_mask;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors_mask &= ~_upwards_motors_mask;
+		_stopped_motors_mask_due_to_flight_phase = 0;
 		break;
 	}
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -81,9 +81,6 @@ private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
-	uint32_t _upwards_motors_mask{};
-	uint32_t _forwards_motors_mask{};
-
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 
 	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -106,7 +106,6 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
 		_forwards_motors_mask = 0;
-		_stopped_motors_mask = 0;
 		break;
 	}
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -216,16 +216,15 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors_mask |= _untiltable_motors;
+		_stopped_motors_mask_due_to_flight_phase = _untiltable_motors;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors_mask = 0;
+		_stopped_motors_mask_due_to_flight_phase = 0;
 		break;
 	}
 }