TailsitterVTOL: allow explicitly stop forward motor with zero thrust

PX4 · Apr 27, 2023 · 9ed24eb · 9ed24eb
1 parent 6a6aaf1
commit 9ed24eb
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 3 deletions.
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
@@ -86,7 +86,27 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
 			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
 		}
 	}
+}
 
+void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	if (matrix_index == 0) {
+		// Stop front facing motors when they are commanded 0 thrust
+		for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+			const uint32_t motor_mask = (1u << actuator_idx);
+
+			if (_fw_motors_mask & motor_mask) {
+				if (actuator_sp(actuator_idx) < .01f) {
+					_stopped_motors |= motor_mask;
+
+				} else {
+					_stopped_motors &= ~motor_mask;
+				}
+			}
+		}
+	}
 }
 
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
@@ -97,15 +117,16 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors //TODO: add option to switch off certain motors in FW
+	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors = 0;
+		_fw_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
+		_fw_motors_mask = 0;
 		_stopped_motors = 0;
 		break;
 	}

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp
@@ -72,16 +72,23 @@ class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public Actuator
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }
 
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
 protected:
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
-	uint32_t _stopped_motors{}; ///< currently stopped motors
+	uint32_t _fw_motors_mask{};
+	uint32_t _stopped_motors{0};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1