[control-allocation] parameterize yaw margin

src/lib/mixer_module/params.c

@@ -16,3 +16,16 @@
  * @group Mixer Output
  */
 PARAM_DEFINE_INT32(MC_AIRMODE, 0);
+
+/**
+ * Multicopter yaw margin percentage.
+ *
+ * The maximum percentage of collective thrust to sacrifice for yaw authority.
+ *
+ * @unit %
+ * @min 0
+ * @max 30
+ * @increment 0.1
+ * @group Mixer Output
+ */
+PARAM_DEFINE_FLOAT(MC_YAW_MARGIN, 15.0f);

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -219,7 +219,7 @@ ControlAllocationSequentialDesaturation::mixYaw()
 	// 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) * 0.15f;
+	_actuator_max += (_actuator_max - _actuator_min) * paramMcYawMargin();
 	desaturateActuators(_actuator_sp, yaw);
 	_actuator_max = max_prev;
 

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp

@@ -57,6 +57,9 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv
 	void allocate() override;
 
 	void updateParameters() override;
+
+	// Normalizes the yaw margin percentage to the range [0, 1].
+	float paramMcYawMargin() const { return _param_mc_yaw_margin_pct.get() / 100.0f; }
 private:
 
 	/**
@@ -123,6 +126,7 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv
 	void mixYaw();
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode   ///< air-mode
+		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,   ///< air-mode
+		(ParamFloat<px4::params::MC_YAW_MARGIN>) _param_mc_yaw_margin_pct
 	);
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp

@@ -325,17 +325,17 @@ TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAn
 	allocator.allocate();
 
 	const auto &actuator_sp = allocator.getActuatorSetpoint();
-	// In the case of yaw saturation, thrust per motor will be reduced by the hard-coded
-	// magic-number yaw margin of 0.15f.
-	constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available.
-	constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR};
+	// In the case of yaw-only saturation, thrust per motor will be reduced by
+	// allocator.paramMcYawMargin().
+	const float yaw_margin{allocator.paramMcYawMargin()};
+	const float yaw_diff_per_motor{1.0f + yaw_margin - DESIRED_THRUST_Z_PER_MOTOR};
 	// At control set point, there will be 2 different actuator values.
-	constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN};
-	constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN};
-	EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
-	EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
-	EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
-	EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	const float high_thrust_z_per_motor{DESIRED_THRUST_Z_PER_MOTOR + yaw_diff_per_motor - allocator.paramMcYawMargin()};
+	const float low_thrust_z_per_motor{DESIRED_THRUST_Z_PER_MOTOR - yaw_diff_per_motor - allocator.paramMcYawMargin()};
+	EXPECT_NEAR(actuator_sp(0), high_thrust_z_per_motor, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(1), high_thrust_z_per_motor, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(2), low_thrust_z_per_motor, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(3), low_thrust_z_per_motor, EXPECT_NEAR_TOL);
 
 	for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
 		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);