feat(control_allocator): Stop motors on NaN thrust setpoint

- Separate stopped motor mask into separate state variables (one for
   stopping due to flight phase, one for stopping due to NaN thrust) and
   combine them on demand in getStoppedMotors. This removes the need of
   hard to follow &= and |= operation involving shared state.
 - Introduce forwards/upwards/sideways motor masks in base
   ActuatorEffecctiveness (and remove previous from individual
   Effectiveness classes), and introduce getSidewaysMotors function
 - Set the masks in the relevant VehicleActuatorEffectiveness classes
 - add setStoppedMotorsBasedOnThrust, which sets the appropriate state
   variables and is called from ControlAllocator
 - use the new nanToZero to clean _thrust_sp, for CA internal use

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -88,3 +88,22 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const
 
 	return total_count;
 }
+
+ActuatorEffectiveness::ActuatorBitmask ActuatorEffectiveness::getStoppedMotors() const
+{
+	// Motors can be stopped (here) for two reasons. They can
+	// additionally be stopped in the ControlAllocator, due to motor
+	// failure.
+
+	//  a) they are turned off because they are generally not used in the given flight phase.
+	ActuatorEffectiveness::ActuatorBitmask 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; }
+
+	if (_sideways_motors_stopped_by_thrust) { stopped_motors_mask |= _sideways_motors_mask; }
+
+	return stopped_motors_mask;
+}

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -205,7 +205,7 @@ public:
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
-	virtual ActuatorBitmask getStoppedMotors() const { return _stopped_motors_mask; }
+	ActuatorBitmask getStoppedMotors() const;
 
 	/**
 	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
@@ -213,8 +213,33 @@ public:
 	 */
 	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
 
+	/**
+	 * Record which components of the thrust setpoint are NaN, so that motors in that direction are stopped.
+	 *
+	 * @param thrust_sp The thrust setpoint as received by the allocator (instance zero - multicopter only)
+	 */
+	void stopMotorsBasedOnThrustSetpoint(const matrix::Vector3f &thrust_sp)
+	{
+		_forwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(0));
+		_sideways_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(1));
+		_upwards_motors_stopped_by_thrust = !PX4_ISFINITE(thrust_sp(2));
+	}
+
 protected:
+
+	// The vehicle effectiveness classes should set these to
+	// ActuatorEffectivenessRotors::get{Upwards,Forwards,Sideways}Motors()
+	// They can alternatively be set to zero to completely disable stopping motors based on NaN thrust
+
+	ActuatorBitmask _upwards_motors_mask{};
+	ActuatorBitmask _forwards_motors_mask{};
+	ActuatorBitmask _sideways_motors_mask{};
+
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
-	ActuatorBitmask _stopped_motors_mask{0};
+	ActuatorBitmask _stopped_motors_mask_due_to_flight_phase{};
+
+	bool _forwards_motors_stopped_by_thrust{false};
+	bool _upwards_motors_stopped_by_thrust{false};
+	bool _sideways_motors_stopped_by_thrust{false};
 
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -395,6 +395,9 @@ ControlAllocator::Run()
 
 	if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
 		_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
+
+		_actuator_effectiveness->stopMotorsBasedOnThrustSetpoint(_thrust_sp);
+		_thrust_sp.nanToZero();
 	}
 
 	if (do_update) {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -64,6 +64,4 @@ private:
 	uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
-
-	ActuatorBitmask _forwards_motors_mask{};
 };

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -59,6 +59,10 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 	_tilts.updateTorqueSign(_mc_rotors.geometry());
 	const bool tilts_added_successfully = _tilts.addActuators(configuration);
 
+	_forwards_motors_mask = _mc_rotors.getForwardsMotors();
+	_sideways_motors_mask = _mc_rotors.getSidewaysMotors();
+	_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+
 	// Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle).
 	// Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated
 	// and reduced to 0 with the sequential desaturation method. Instead we add it after.

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -52,5 +52,9 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configura
 	// Motors
 	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
+	_forwards_motors_mask = _mc_rotors.getForwardsMotors();
+	_sideways_motors_mask = _mc_rotors.getSidewaysMotors();
+	_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+
 	return rotors_added_successfully;
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -267,6 +267,11 @@ ActuatorBitmask ActuatorEffectivenessRotors::getMotors() const
 	return motors;
 }
 
+// These three allow an angle deviation of 8.1 deg. Motor tilt for yaw actuation
+// could reasonably be more. Alternatively, we could classify the motors such
+// that every rotor points in a defined direction, putting the decision
+// boundaries at 45 deg. TODO revisit
+
 ActuatorBitmask ActuatorEffectivenessRotors::getUpwardsMotors() const
 {
 	ActuatorBitmask upwards_motors = 0;
@@ -297,6 +302,22 @@ ActuatorBitmask ActuatorEffectivenessRotors::getForwardsMotors() const
 	return forward_motors;
 }
 
+ActuatorBitmask ActuatorEffectivenessRotors::getSidewaysMotors() const
+{
+	ActuatorBitmask sideways_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		// This includes both left and right pointing motors, in contrast to the other two directions...
+		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) > 0.5f && fabsf(axis(2)) < 0.1f) {
+			sideways_motors |= 1u << i;
+		}
+	}
+
+	return sideways_motors;
+}
+
 bool
 ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -129,8 +129,10 @@ public:
 	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
 
 	ActuatorBitmask getMotors() const;
+
 	ActuatorBitmask getUpwardsMotors() const;
 	ActuatorBitmask getForwardsMotors() const;
+	ActuatorBitmask getSidewaysMotors() const;
 
 private:
 	void updateParams() override;

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;
 
-	ActuatorBitmask _upwards_motors_mask{};
-	ActuatorBitmask _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

@@ -59,6 +59,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 	// enable MC yaw control if more than 3 rotors
 	_mc_rotors.enableYawByDifferentialThrust(_mc_rotors.geometry().num_rotors > 3);
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
+	updateMotorMasks();
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
@@ -96,17 +97,29 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors
-	switch (flight_phase) {
+	updateMotorMasks();
+
+}
+
+void ActuatorEffectivenessTailsitterVTOL::updateMotorMasks()
+{
+	switch (_flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
+
+		// TODO only set this mask if the motors are not needed for rate control, watching one or both of:
+		//  - _mc_rotors.geometry().num_rotors > 3 (used in getEffectivenessMatrix to enable MC yaw control)
+		//  - VT_FW_DIFTHR_EN (the general switch for this -- are the two equivalent?)
+
 		_forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards
+		_upwards_motors_mask = 0;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
 		_forwards_motors_mask = 0;
-		_stopped_motors_mask = 0;
+		_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
+
 		break;
 	}
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -77,11 +77,11 @@ public:
 	const char *name() const override { return "VTOL Tailsitter"; }
 
 protected:
+	void updateMotorMasks();
+
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
-	ActuatorBitmask _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/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -83,6 +83,8 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 			     << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
+	_forwards_motors_mask = _mc_rotors.getForwardsMotors();
+	_upwards_motors_mask = _mc_rotors.getUpwardsMotors();
 	_motors = _mc_rotors.getMotors();
 
 	// Control Surfaces
@@ -216,16 +218,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;
 	}
 }