[WIP] Limited airmode

CMakeLists.txt

@@ -420,6 +420,7 @@ if(BUILD_TESTING)
 			DEPENDS
 				px4
 				examples__dyn_hello
+				test_mixer_multirotor
 			USES_TERMINAL
 			COMMENT "Running tests"
 			WORKING_DIRECTORY ${PX4_BINARY_DIR})

src/lib/CMakeLists.txt

@@ -57,6 +57,7 @@ add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
+add_subdirectory(mixer)
 add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -62,11 +62,10 @@ enum class ActuatorType {
 
 enum class EffectivenessUpdateReason {
 	NO_EXTERNAL_UPDATE = 0,
-	CONFIGURATION_UPDATE = 1,
-	MOTOR_ACTIVATION_UPDATE = 2,
+	CONFIGURATION_UPDATE = 1, ///< config changes (parameter)
+	MOTOR_ACTIVATION_UPDATE = 2, ///< motor failure detected or certain redundant motors are switched off to save energy
 };
 
-
 class ActuatorEffectiveness
 {
 public:
@@ -196,9 +195,8 @@ class ActuatorEffectiveness
 	 * It is called after the matrix multiplication and before final clipping.
 	 * @param actuator_sp input & output setpoint
 	 */
-	virtual 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) {}
+	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+				    ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) {}
 
 	/**
 	 * Get a bitmask of motors to be stopped

src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

@@ -41,5 +41,4 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
-		PID
 )

src/lib/control_allocation/control_allocation/CMakeLists.txt

@@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ControlAllocation PRIVATE mathlib)
 
 px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
-px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
+px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation VehicleActuatorEffectiveness)

src/lib/control_allocation/control_allocation/ControlAllocation.hpp

@@ -82,7 +82,7 @@ class ControlAllocation
 	static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS;
 	static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES;
 
-	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
+	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
 
 	enum ControlAxis {
 		ROLL = 0,
@@ -121,7 +121,7 @@ class ControlAllocation
 	 *
 	 * @return Actuator vector
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorSetpoint() const { return _actuator_sp; }
+	const ActuatorVector &getActuatorSetpoint() const { return _actuator_sp; }
 
 	/**
 	 * Set the desired control vector
@@ -157,28 +157,28 @@ class ControlAllocation
 	 *
 	 * @param actuator_min Minimum actuator values
 	 */
-	void setActuatorMin(const matrix::Vector<float, NUM_ACTUATORS> &actuator_min) { _actuator_min = actuator_min; }
+	void setActuatorMin(const ActuatorVector &actuator_min) { _actuator_min = actuator_min; }
 
 	/**
 	 * Get the minimum actuator values
 	 *
 	 * @return Minimum actuator values
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMin() const { return _actuator_min; }
+	const ActuatorVector &getActuatorMin() const { return _actuator_min; }
 
 	/**
 	 * Set the maximum actuator values
 	 *
 	 * @param actuator_max Maximum actuator values
 	 */
-	void setActuatorMax(const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) { _actuator_max = actuator_max; }
+	void setActuatorMax(const ActuatorVector &actuator_max) { _actuator_max = actuator_max; }
 
 	/**
 	 * Get the maximum actuator values
 	 *
 	 * @return Maximum actuator values
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMax() const { return _actuator_max; }
+	const ActuatorVector &getActuatorMax() const { return _actuator_max; }
 
 	/**
 	 * Set the current actuator setpoint.
@@ -189,9 +189,9 @@ class ControlAllocation
 	 *
 	 * @param actuator_sp Actuator setpoint
 	 */
-	void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
+	void setActuatorSetpoint(const ActuatorVector &actuator_sp);
 
-	void setSlewRateLimit(const matrix::Vector<float, NUM_ACTUATORS> &slew_rate_limit)
+	void setSlewRateLimit(const ActuatorVector &slew_rate_limit)
 	{ _actuator_slew_rate_limit = slew_rate_limit; }
 
 	/**
@@ -206,7 +206,7 @@ class ControlAllocation
 	 *
 	 * @param actuator Actuator vector to clip
 	 */
-	void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
+	void clipActuatorSetpoint(ActuatorVector &actuator) const;
 
 	void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
 
@@ -219,7 +219,7 @@ class ControlAllocation
 	 *
 	 * @return Clipped actuator setpoint
 	 */
-	matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
+	ActuatorVector normalizeActuatorSetpoint(const ActuatorVector &actuator)
 	const;
 
 	virtual void updateParameters() {}
@@ -233,12 +233,12 @@ class ControlAllocation
 
 	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  ///< Effectiveness matrix
 	matrix::Vector<float, NUM_AXES> _control_allocation_scale;  	///< Scaling applied during allocation
-	matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; 	///< Neutral actuator values
-	matrix::Vector<float, NUM_ACTUATORS> _actuator_min; 	///< Minimum actuator values
-	matrix::Vector<float, NUM_ACTUATORS> _actuator_max; 	///< Maximum actuator values
-	matrix::Vector<float, NUM_ACTUATORS> _actuator_slew_rate_limit; 	///< Slew rate limit
-	matrix::Vector<float, NUM_ACTUATORS> _prev_actuator_sp;  	///< Previous actuator setpoint
-	matrix::Vector<float, NUM_ACTUATORS> _actuator_sp;  	///< Actuator setpoint
+	ActuatorVector _actuator_trim; 	///< Neutral actuator values
+	ActuatorVector _actuator_min; 	///< Minimum actuator values
+	ActuatorVector _actuator_max; 	///< Maximum actuator values
+	ActuatorVector _actuator_slew_rate_limit; 	///< Slew rate limit
+	ActuatorVector _prev_actuator_sp;  	///< Previous actuator setpoint
+	ActuatorVector _actuator_sp;  	///< Actuator setpoint
 	matrix::Vector<float, NUM_AXES> _control_sp;   		///< Control setpoint
 	matrix::Vector<float, NUM_AXES> _control_trim; 		///< Control at trim actuator values
 	int _num_actuators{0};

src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp

@@ -36,10 +36,11 @@
  *
  * @author Roman Bapst <bapstroman@gmail.com>
  * @author Beat Küng <beat-kueng@gmx.net>
+ * @author Matthias Grob <maetugr@gmail.com>
  */
 
 #include "ControlAllocationSequentialDesaturation.hpp"
-
+#include "mathlib/mathlib.h"
 
 void
 ControlAllocationSequentialDesaturation::allocate()
@@ -51,38 +52,39 @@ ControlAllocationSequentialDesaturation::allocate()
 
 	switch (_param_mc_airmode.get()) {
 	case 1:
-		mixAirmodeRP();
+		mix(1.f, 0.f);
 		break;
 
 	case 2:
-		mixAirmodeRPY();
+		mix(1.f, 1.f);
 		break;
 
 	default:
-		mixAirmodeDisabled();
+		mix(0.f, 0.f);
 		break;
 	}
 }
 
-void ControlAllocationSequentialDesaturation::desaturateActuators(
+float ControlAllocationSequentialDesaturation::desaturateActuators(
 	ActuatorVector &actuator_sp,
-	const ActuatorVector &desaturation_vector, bool increase_only)
+	const ActuatorVector &desaturation_vector,
+	float increase_limit, float decrease_limit)
 {
 	float gain = computeDesaturationGain(desaturation_vector, actuator_sp);
-
-	if (increase_only && gain < 0.f) {
-		return;
-	}
+	ActuatorVector u1 = actuator_sp;
 
 	for (int i = 0; i < _num_actuators; i++) {
-		actuator_sp(i) += gain * desaturation_vector(i);
+		u1(i) += gain * desaturation_vector(i);
 	}
 
-	gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp);
+	gain += 0.5f * computeDesaturationGain(desaturation_vector, u1);
+	gain = math::constrain(gain, -increase_limit, decrease_limit);
 
 	for (int i = 0; i < _num_actuators; i++) {
 		actuator_sp(i) += gain * desaturation_vector(i);
 	}
+
+	return gain;
 }
 
 float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
@@ -92,7 +94,7 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	float k_max = 0.f;
 
 	for (int i = 0; i < _num_actuators; i++) {
-		// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
+		// Do not try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
 		if (fabsf(desaturation_vector(i)) < 0.2f) {
 			continue;
 		}
@@ -118,113 +120,61 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	return k_min + k_max;
 }
 
-void
-ControlAllocationSequentialDesaturation::mixAirmodeRP()
+void ControlAllocationSequentialDesaturation::mix(float roll_pitch_limit, float yaw_limit)
 {
-	// Airmode for roll and pitch, but not yaw
-
-	// Mix without yaw
+	ActuatorVector roll;
+	ActuatorVector pitch;
+	ActuatorVector yaw;
 	ActuatorVector thrust_z;
 
+	ActuatorVector mixed_with_yaw;
+
 	for (int i = 0; i < _num_actuators; i++) {
 		_actuator_sp(i) = _actuator_trim(i) +
 				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
 				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  //_mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
 				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
 				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
 				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		mixed_with_yaw(i) = _actuator_sp(i) + _mix(i,
+				    ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
+		roll(i) = _mix(i, ControlAxis::ROLL);
+		pitch(i) = _mix(i, ControlAxis::PITCH);
+		yaw(i) = _mix(i, ControlAxis::YAW);
 		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
 	}
 
-	desaturateActuators(_actuator_sp, thrust_z);
-
-	// Mix yaw independently
-	mixYaw();
-}
-
-void
-ControlAllocationSequentialDesaturation::mixAirmodeRPY()
-{
-	// Airmode for roll, pitch and yaw
+	const bool debug = true;
 
-	// Do full mixing
-	ActuatorVector thrust_z;
-	ActuatorVector yaw;
+	if (debug) { printf("Roll Pitch Thrust Mixed\n"); _actuator_sp.print(); };
 
-	for (int i = 0; i < _num_actuators; i++) {
-		_actuator_sp(i) = _actuator_trim(i) +
-				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
-				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
-				  _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
-				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
-				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
-				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
-		yaw(i) = _mix(i, ControlAxis::YAW);
-	}
+	if (debug) { printf("Roll Pitch Yaw Thrust Mixed\n"); mixed_with_yaw.print(); };
 
-	desaturateActuators(_actuator_sp, thrust_z);
+	float rp = desaturateActuators(_actuator_sp, thrust_z, roll_pitch_limit);
 
-	// Unsaturate yaw (in case upper and lower bounds are exceeded)
-	// to prioritize roll/pitch over yaw.
-	desaturateActuators(_actuator_sp, yaw);
-}
+	float rpy = desaturateActuators(mixed_with_yaw, thrust_z, roll_pitch_limit);
 
-void
-ControlAllocationSequentialDesaturation::mixAirmodeDisabled()
-{
-	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
+	printf("%.3f %.3f\n", (double)rp, (double)rpy);
 
-	// Mix without yaw
-	ActuatorVector thrust_z;
-	ActuatorVector roll;
-	ActuatorVector pitch;
+	const bool use_rpy = fabsf(rpy) < fabsf(rp);
 
-	for (int i = 0; i < _num_actuators; i++) {
-		_actuator_sp(i) = _actuator_trim(i) +
-				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
-				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
-				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
-				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
-				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
-		roll(i) = _mix(i, ControlAxis::ROLL);
-		pitch(i) = _mix(i, ControlAxis::PITCH);
+	if (use_rpy) {
+		_actuator_sp = mixed_with_yaw;
 	}
 
-	// only reduce thrust
-	desaturateActuators(_actuator_sp, thrust_z, true);
-
-	// Reduce roll/pitch acceleration if needed to unsaturate
 	desaturateActuators(_actuator_sp, roll);
 	desaturateActuators(_actuator_sp, pitch);
 
-	// Mix yaw independently
-	mixYaw();
-}
-
-void
-ControlAllocationSequentialDesaturation::mixYaw()
-{
-	// Add yaw to outputs
-	ActuatorVector yaw;
-	ActuatorVector thrust_z;
+	if (!use_rpy) {
+		for (int i = 0; i < _num_actuators; i++) {
+			_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
+		}
 
-	for (int i = 0; i < _num_actuators; i++) {
-		_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
-		yaw(i) = _mix(i, ControlAxis::YAW);
-		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+		desaturateActuators(_actuator_sp, thrust_z, yaw_limit, 0.15f);
 	}
 
-	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
-	// and allow some yaw response at maximum thrust
-	ActuatorVector max_prev = _actuator_max;
-	_actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN;
 	desaturateActuators(_actuator_sp, yaw);
-	_actuator_max = max_prev;
-
-	// reduce thrust only
-	desaturateActuators(_actuator_sp, thrust_z, true);
 }
 
 void