AttitudeControl: let the user set yaw weight directly via parameter

instead of infering the yaw weight from the gains which can lead to
unexpected results depending on the particular vehicle tuning.

src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp

@@ -37,20 +37,19 @@
 
 #include <AttitudeControl.hpp>
 
-#include <mathlib/math/Limits.hpp>
 #include <mathlib/math/Functions.hpp>
 
 using namespace matrix;
 
-void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain)
+void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight)
 {
 	_proportional_gain = proportional_gain;
+	_yaw_w = math::constrain(yaw_weight, 0.f, 1.f);
 
-	// prepare yaw weight from the ratio between roll/pitch and yaw gains
-	const float roll_pitch_gain = (proportional_gain(0) + proportional_gain(1)) / 2.f;
-	_yaw_w = math::constrain(proportional_gain(2) / roll_pitch_gain, 0.f, 1.f);
-
-	_proportional_gain(2) = roll_pitch_gain;
+	// compensate for the effect of the yaw weight rescaling the output
+	if (_yaw_w > 1e-4f) {
+		_proportional_gain(2) /= _yaw_w;
+	}
 }
 
 matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, const float yawspeed_feedforward)

src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp

@@ -49,6 +49,7 @@
 #pragma once
 
 #include <matrix/matrix/math.hpp>
+#include <mathlib/math/Limits.hpp>
 
 class AttitudeControl
 {
@@ -59,8 +60,9 @@ public:
 	/**
 	 * Set proportional attitude control gain
 	 * @param proportional_gain 3D vector containing gains for roll, pitch, yaw
+	 * @param yaw_weight A fraction [0,1] deprioritizing yaw compared to roll and pitch
 	 */
-	void setProportionalGain(const matrix::Vector3f &proportional_gain);
+	void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight);
 
 	/**
 	 * Set hard limit for output rate setpoints
@@ -80,5 +82,5 @@ public:
 private:
 	matrix::Vector3f _proportional_gain;
 	matrix::Vector3f _rate_limit;
-	float _yaw_w = 0.0f; /**< yaw weight [0,1] to prioritize roll and pitch */
+	float _yaw_w{0.f}; /**< yaw weight [0,1] to prioritize roll and pitch */
 };