diff --git a/src/lib/mixer_module/params.c b/src/lib/mixer_module/params.c index e54a25eca0ac..959e7038fae2 100644 --- a/src/lib/mixer_module/params.c +++ b/src/lib/mixer_module/params.c @@ -16,3 +16,17 @@ * @group Mixer Output */ PARAM_DEFINE_INT32(MC_AIRMODE, 0); + +/** + * Multicopter yaw margin percentage. + * + * The maximum percentage of collective thrust to sacrifice for yaw authority. + * Note that this parameter is ignored when airmode is enabled for yaw. + * + * @unit % + * @min 0 + * @max 30 + * @increment 0.1 + * @group Mixer Output + */ +PARAM_DEFINE_FLOAT(MC_YAW_MARGIN, 15.0f); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 83be2a3b337b..14a8b815cf68 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/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; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp index 53c422cd3940..1cdf4b6424a1 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp +++ b/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) _param_mc_airmode ///< air-mode + (ParamInt) _param_mc_airmode, ///< air-mode + (ParamFloat) _param_mc_yaw_margin_pct ); }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp index 60392330c939..171136f47761 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/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);