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
+	// compensate for the effect of the yaw weight rescaling the output
-	const float roll_pitch_gain = (proportional_gain(0) + proportional_gain(1)) / 2.f;
+	if (_yaw_w > 1e-4f) {
-	_yaw_w = math::constrain(proportional_gain(2) / roll_pitch_gain, 0.f, 1.f);
+		_proportional_gain(2) /= _yaw_w;
-
+	}
-	_proportional_gain(2) = roll_pitch_gain;
 }
 
 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 */
 };

src/modules/mc_att_control/mc_att_control.hpp

@@ -139,6 +139,7 @@ private:
 		(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
 		(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
 		(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
+		(ParamFloat<px4::params::MC_YAW_WEIGHT>) _param_mc_yaw_weight,
 
 		(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
 		(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -94,7 +94,8 @@ void
 MulticopterAttitudeControl::parameters_updated()
 {
 	// Store some of the parameters in a more convenient way & precompute often-used values
-	_attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()));
+	_attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()),
+					      _param_mc_yaw_weight.get());
 
 	// angular rate limits
 	using math::radians;

src/modules/mc_att_control/mc_att_control_params.c

@@ -81,6 +81,25 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
  */
 PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f);
 
+/**
+ * Yaw weight
+ *
+ * A fraction [0,1] deprioritizing yaw compared to roll and pitch in non-linear attitude control.
+ * Deprioritizing yaw is necessary because multicopters have much less control authority
+ * in yaw compared to the other axes and it makes sense because yaw is not critical for
+ * stable hovering or 3D navigation.
+ *
+ * For yaw control tuning use MC_YAW_P. This ratio has no inpact on the yaw gain.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.1
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAW_WEIGHT, 0.4f);
+
 /**
  * Max roll rate
  *