feat(fw_att_control): yaw error reallocation

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -75,6 +75,7 @@ FixedwingAttitudeControl::parameters_update()
 	_proportional_gain = matrix::Vector3f(1.0f / _param_fw_r_tc.get(),
 					      1.0f / _param_fw_p_tc.get(),
 					      0.0f);
+	_yaw_w = _param_fw_yaw_weight.get();
 
 	_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
 	_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
@@ -85,7 +86,7 @@ FixedwingAttitudeControl::parameters_update()
 }
 
 void
-FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float roll)
+FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 {
 	if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
 
@@ -103,7 +104,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float
 				pitch_body = constrain(pitch_body,
 						       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
 
-				float yaw_sp = yaw_body + sinf(roll) * 0.0f;
+				float yaw_sp = yaw_body;
 
 				const Quatf q(Eulerf(roll_body, pitch_body, yaw_sp));
 				q.copyTo(_att_sp.q_d);
@@ -254,7 +255,7 @@ void FixedwingAttitudeControl::Run()
 
 		const matrix::Eulerf euler_angles(_R);
 
-		vehicle_manual_poll(euler_angles.psi(), euler_angles.phi());
+		vehicle_manual_poll(euler_angles.psi());
 
 		vehicle_attitude_setpoint_poll();
 
@@ -297,14 +298,13 @@ void FixedwingAttitudeControl::Run()
 
 				if (q_sp.isAllFinite()) {
 					Quatf q_current(att.q);
-
 					Quatf q_err = (q_current.inversed() * q_sp).canonical();
 					Vector3f e = 2.f * q_err.imag();
 
 					Vector3f body_rates_setpoint;
 					body_rates_setpoint(0) = _proportional_gain(0) * e(0);
-					body_rates_setpoint(1) = _proportional_gain(1) * e(1);
-					body_rates_setpoint(2) = 0.f; // yaw handled later
+					body_rates_setpoint(1) = _proportional_gain(1) * e(1) - 6.f * _yaw_w * e(2) * tanf(euler_angles.phi());
+					body_rates_setpoint(2) = 0.f; // yaw handled through turn coordination and manual yaw input
 
 					// Turn coordination
 					const float V = math::max(get_airspeed_constrained(), 0.1f);
@@ -312,7 +312,6 @@ void FixedwingAttitudeControl::Run()
 
 					body_rates_setpoint(2) = r_tc_ff;
 
-
 					body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get()));
 					body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get()));
 					body_rates_setpoint(2) = math::constrain(body_rates_setpoint(2), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -157,15 +157,17 @@ private:
 		(ParamFloat<px4::params::FW_WR_IMAX>) _param_fw_wr_imax,
 		(ParamFloat<px4::params::FW_WR_P>) _param_fw_wr_p,
 
+		(ParamFloat<px4::params::FW_YAW_WEIGHT>) _param_fw_yaw_weight,
 		(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
 		(ParamFloat<px4::params::FW_MAN_YR_MAX>) _param_man_yr_max
 	)
 
 	matrix::Vector3f _proportional_gain;
+	float _yaw_w{1.0f};
 	WheelController _wheel_ctrl;
 
 	void parameters_update();
-	void vehicle_manual_poll(const float yaw_body, const float roll);
+	void vehicle_manual_poll(const float yaw_body);
 	void vehicle_attitude_setpoint_poll();
 	void vehicle_land_detected_poll();
 	float get_airspeed_constrained();

src/modules/fw_att_control/fw_att_control_params.c

@@ -197,6 +197,21 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f);
  */
 PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
 
+/**
+ * Yaw weight
+ *
+ * Scales yaw-error correction through the pitch-rate setpoint in non-linear attitude control.
+ *
+ * For yaw control tuning use FW_Y_RMAX and rate controller tuning. This ratio has no impact on yaw gain.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YAW_WEIGHT, 1.0f);
+
 /**
  * Pitch setpoint offset (pitch at level flight)
  *

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -299,7 +299,7 @@ void FwLateralLongitudinalControl::Run()
 			float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
 			float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
 
-			float yaw_body = _yaw + sinf(roll_body) * 0.45f;
+			float yaw_body = _yaw;
 
 			const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
 
@@ -610,7 +610,6 @@ void FwLateralLongitudinalControl::updateAttitude() {
 		const Eulerf euler_angles(R);
 		_long_control_state.pitch_rad = euler_angles.theta();
 		_yaw = euler_angles.psi();
-		_roll = euler_angles.phi();
 
 		// load factor due to banking
 		const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp

@@ -192,7 +192,6 @@ private:
 	hrt_abstime _time_wind_last_received{0};
 	SlewRate<float> _roll_slew_rate;
 	float _yaw{0.f};
-	float _roll{0.f};
 	struct lateral_control_state {
 		matrix::Vector2f ground_speed;
 		matrix::Vector2f wind_speed;