feat(fw_att_control): tilt-tracking controller

msg/CMakeLists.txt

@@ -276,7 +276,6 @@ set(msg_files
 	versioned/VehicleLocalPosition.msg
 	versioned/VehicleOdometry.msg
 	versioned/VehicleRatesSetpoint.msg
-	versioned/VehicleEulerRatesSetpoint.msg
 	versioned/VehicleStatus.msg
 	versioned/VtolVehicleStatus.msg
 	versioned/Wind.msg

msg/versioned/VehicleEulerRatesSetpoint.msg

@@ -1,5 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-
-float32 roll_rate		# body frame roll rate setpoint (rad/s)
-float32 pitch_rate		# body frame pitch rate setpoint (rad/s)
-float32 yaw_rate		# body frame yaw rate setpoint (rad/s)

src/lib/matrix/matrix/Quaternion.hpp

@@ -518,6 +518,48 @@ public:
 		return Vector3<Type>(q(1), q(2), q(3));
 	}
 
+	/**
+	 * Corresponding body x-axis to an attitude quaternion /
+	 * first orthogonal unit basis vector
+	 *
+	 * == first column of the equivalent rotation matrix
+	 * but calculated more efficiently than a full conversion
+	 */
+	Vector3<Type> dcm_x() const
+	{
+		const Quaternion &q = *this;
+		Vector3<Type> R_x;
+		const Type a = q(0);
+		const Type b = q(1);
+		const Type c = q(2);
+		const Type d = q(3);
+		R_x(0) = a * a + b * b - c * c - d * d;
+		R_x(1) = 2 * (b * c + a * d);
+		R_x(2) = 2 * (b * d - a * c);
+		return R_x;
+	}
+
+	/**
+	 * Corresponding body y-axis to an attitude quaternion /
+	 * second orthogonal unit basis vector
+	 *
+	 * == second column of the equivalent rotation matrix
+	 * but calculated more efficiently than a full conversion
+	 */
+	Vector3<Type> dcm_y() const
+	{
+		const Quaternion &q = *this;
+		Vector3<Type> R_y;
+		const Type a = q(0);
+		const Type b = q(1);
+		const Type c = q(2);
+		const Type d = q(3);
+		R_y(0) = 2 * (b * c - a * d);
+		R_y(1) = a * a - b * b + c * c - d * d;
+		R_y(2) = 2 * (c * d + a * b);
+		return R_y;
+	}
+
 	/**
 	 * Corresponding body z-axis to an attitude quaternion /
 	 * last orthogonal unit basis vector

src/modules/fw_att_control/CMakeLists.txt

@@ -38,10 +38,7 @@ px4_add_module(
 		FixedwingAttitudeControl.cpp
 		FixedwingAttitudeControl.hpp
 
-		fw_pitch_controller.cpp
-		fw_roll_controller.cpp
 		fw_wheel_controller.cpp
-		fw_yaw_controller.cpp
 	DEPENDS
 		px4_work_queue
 	)

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -72,25 +72,10 @@ FixedwingAttitudeControl::init()
 void
 FixedwingAttitudeControl::parameters_update()
 {
-	_proportional_gain = matrix::Vector3f(1.0f / _param_fw_p_tc.get(),
+	_proportional_gain = matrix::Vector3f(1.0f / _param_fw_r_tc.get(),
 					      1.0f / _param_fw_p_tc.get(),
 					      0.0f);
 
-	_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
-	_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
-
-	_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
-	_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
-	_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
-
-	_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
-
-
-	_max_roll_rate = radians(_param_fw_r_rmax.get());
-	_max_pitch_rate_pos = radians(_param_fw_p_rmax_pos.get());
-	_max_pitch_rate_neg = radians(_param_fw_p_rmax_neg.get());
-	_max_yaw_rate = radians(_param_fw_y_rmax.get());
-
 	_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
 	_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
 	_wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
@@ -100,10 +85,8 @@ FixedwingAttitudeControl::parameters_update()
 }
 
 void
-FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
+FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 {
-	const matrix::Eulerf euler_angles(R);
-	float yaw_body = euler_angles.psi();
 	if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
 
 		// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
@@ -111,7 +94,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
 
 			if (!_vcontrol_mode.flag_control_climb_rate_enabled && _vcontrol_mode.flag_control_attitude_enabled) {
 
-				// STABILIZED mode generate the attitude setpoint from manual user inputs
+				// STABILIZED mode: setpoint generation
 
 				const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
 
@@ -120,11 +103,11 @@ FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
 				pitch_body = constrain(pitch_body,
 						       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
 
-				_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
-
 				const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
 				q.copyTo(_att_sp.q_d);
 
+				_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
+
 				_att_sp.timestamp = hrt_absolute_time();
 
 				_attitude_sp_pub.publish(_att_sp);
@@ -266,9 +249,10 @@ void FixedwingAttitudeControl::Run()
 			/* fill in new attitude data */
 			_R = R_adapted;
 		}
+
 		const matrix::Eulerf euler_angles(_R);
 
-		vehicle_manual_poll(_R);
+		vehicle_manual_poll(euler_angles.psi());
 
 		vehicle_attitude_setpoint_poll();
 
@@ -311,91 +295,61 @@ void FixedwingAttitudeControl::Run()
 
 				if (q_sp.isAllFinite()) {
 
-
-					// Current attitude
 					const Quatf q_current(att.q);
+					const Quatf q_sp_full(q_sp);
+					const Vector3f ez_world(0.f, 0.f, 1.f);
+					const Vector3f ex_c = q_current.dcm_x();
+					const Vector3f ez_c = q_current.dcm_z();
+					const Vector3f ez_sp = q_sp_full.dcm_z();
 
-					// Full desired attitude (setpoint)
+					// Tilt-only error quaternion
-					const Quatf qd_full = q_sp;
+					Quatf q_tilt_err(ez_c, ez_sp);
 
-					// --- Scheduling: 0 = normal FW, 1 = near-vertical/full-3D ---
+					// Reduced desired attitude
-					const float pitch = Eulerf(q_current).theta(); // scheduling only
+					Quatf q_des_red = (q_tilt_err * q_current).normalized();
-					const float s = math::constrain((fabsf(pitch) - radians(60.f)) / radians(20.f), 0.f, 1.f); // 0..1
+					Quatf q_err = (q_current.inversed() * q_des_red).canonical();
+					Vector3f e = 2.f * q_err.imag();
 
-					// --- Build reduced desired attitude qd_red: align "tilt" (body Z axis) but don't chase yaw ---
+					Vector3f body_rates_setpoint;
-					// Note: assumes Quatf::dcm_z() returns body Z axis expressed in world/NED (same convention as PX4 MC).
+					body_rates_setpoint(0) = _proportional_gain(0) * e(0);
-					const Vector3f ez_current = q_current.dcm_z(); // current body Z in world
+					body_rates_setpoint(1) = _proportional_gain(1) * e(1) * 2.5f;   // multiplied by 2.5 to maintain the gains
-					const Vector3f ez_desired = qd_full.dcm_z();   // desired body Z in world
+					body_rates_setpoint(2) = 0.f;
 
-					// Quaternion rotating ez_current -> ez_desired (tilt correction only)
+					// Turn coordination
-					Quatf q_tilt_err(ez_current, ez_desired);
+					const float V = math::max(get_airspeed_constrained(), 0.1f);
 
-					// Convert that tilt error into an absolute reduced desired attitude.
+					Vector3f x_h = ex_c - ez_world * ex_c.dot(ez_world);   // forward projected to horizontal
-					// This matches the PX4 multicopter pattern (right multiplication).
+					float r_tc_ff = 0.f;
-					Quatf qd_red;
 
-					if (fabsf(q_tilt_err(1)) > (1.f - 1e-5f) || fabsf(q_tilt_err(2)) > (1.f - 1e-5f)) {
+					const float xhn = x_h.norm();
-						// Corner case: vectors almost exactly opposite (180 deg). Tilt-only separation is ill-defined.
-						// Fall back to full desired attitude; it's safe/stable.
-						qd_red = qd_full;
 
-					} else {
+					if (xhn > 1e-3f) {
-						// Apply tilt correction to current attitude to get a reduced desired attitude
+
-						qd_red = q_tilt_err * q_current;
+						x_h /= xhn;
+						Vector3f y_h = ez_world.cross(x_h);
+						const float yhn = y_h.norm();
+
+						if (yhn > 1e-6f) {
+
+							y_h /= yhn;
+							const float cos_tilt = ez_c.dot(ez_world);
+							const float sin_bank = ez_c.dot(y_h);
+
+							float tan_bank = 0.f;
+
+							if (fabsf(cos_tilt) > 0.1f) {
+								tan_bank = sin_bank / cos_tilt;
+							}
+
+							r_tc_ff = (9.81f / V) * tan_bank * 0.6f;
+						}
 					}
 
-					// --- Compute rate setpoint A: tilt-only attitude tracking (normal FW) ---
+					body_rates_setpoint(2) = -r_tc_ff;
-
-					// qe_tilt is rotation from current to reduced desired
-					const Quatf qe_tilt = (q_current.inversed() * qd_red).canonical();
-
-					// Use bounded quaternion vector part as error (stable & well-tuned pattern)
-					const Vector3f e_tilt = 2.f * qe_tilt.imag();
-
-					Vector3f rates_tilt = e_tilt.emult(_proportional_gain);
-
-					// In normal FW flight, yaw is NOT attitude-tracked; use existing yaw/turn coordination
-					const float yaw_rate_tc = _yaw_ctrl.get_body_rate_setpoint();
-					rates_tilt(2) = yaw_rate_tc;
-
-					// --- Compute rate setpoint B: full 3D attitude tracking (near-vertical) ---
-					const Quatf qe_full = (q_current.inversed() * qd_full).canonical();
-					const Vector3f e_full = 2.f * qe_full.imag();
-					Vector3f rates_full = e_full.emult(_proportional_gain);
-
-					// --- Blend A and B ---
-					Vector3f body_rates_setpoint = (1.f - s) * rates_tilt + s * rates_full;
-
-
-					// limit rates (kept identical)
-					body_rates_setpoint(0) = constrain(body_rates_setpoint(0), -_max_roll_rate, _max_roll_rate);
-					body_rates_setpoint(1) = constrain(body_rates_setpoint(1), -_max_pitch_rate_neg, _max_pitch_rate_pos);
-					body_rates_setpoint(2) = constrain(body_rates_setpoint(2), -_max_yaw_rate, _max_yaw_rate);
-
-
-					///////////////////////////////////
-					const Eulerf euler_sp(q_sp);
-					const float roll_sp = euler_sp.phi();
-					const float pitch_sp = euler_sp.theta();
-
-					_roll_ctrl.control_roll(roll_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
-								euler_angles.theta());
-					_pitch_ctrl.control_pitch(pitch_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
-								  euler_angles.theta());
-					_yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
-							      euler_angles.theta(), get_airspeed_constrained());
-
-					/* Update input data for rate controllers */
-					Vector3f _euler_body_rates_sp = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
-									_yaw_ctrl.get_body_rate_setpoint());
-
-					_euler_rates_sp.timestamp = hrt_absolute_time();
-					_euler_rates_sp.roll_rate = _euler_body_rates_sp(0);
-					_euler_rates_sp.pitch_rate = _euler_body_rates_sp(1);
-					_euler_rates_sp.yaw_rate = _euler_body_rates_sp(2);
-					///////////////////////////////////
-
 
+					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()));
 
 					autotune_attitude_control_status_s pid_autotune;
 					matrix::Vector3f bodyrate_autotune_ff;
@@ -434,7 +388,6 @@ void FixedwingAttitudeControl::Run()
 					_rates_sp.timestamp = hrt_absolute_time();
 
 					_rate_sp_pub.publish(_rates_sp);
-					_euler_rates_sp_pub.publish(_euler_rates_sp);
 				}
 			}
 		}

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -34,10 +34,8 @@
 #pragma once
 
 #include <drivers/drv_hrt.h>
-#include "fw_pitch_controller.h"
+
-#include "fw_roll_controller.h"
 #include "fw_wheel_controller.h"
-#include "fw_yaw_controller.h"
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
@@ -65,7 +63,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_euler_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 
 using matrix::Eulerf;
@@ -116,14 +113,12 @@ private:
 
 	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub;
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
-	uORB::Publication<vehicle_euler_rates_setpoint_s> _euler_rates_sp_pub{ORB_ID(vehicle_euler_rates_setpoint)};
 	uORB::Publication<landing_gear_wheel_s>		_landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)};
 
 	manual_control_setpoint_s		_manual_control_setpoint{};
 	vehicle_attitude_setpoint_s		_att_sp{};
 	vehicle_control_mode_s			_vcontrol_mode{};
 	vehicle_rates_setpoint_s		_rates_sp{};
-	vehicle_euler_rates_setpoint_s		_euler_rates_sp{};
 	vehicle_status_s			_vehicle_status{};
 	landing_gear_wheel_s			_landing_gear_wheel{};
 
@@ -164,24 +159,14 @@ private:
 
 		(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;
-	RollController _roll_ctrl;
-	PitchController _pitch_ctrl;
-	YawController _yaw_ctrl;
-
-	float _max_roll_rate;
-	float _max_pitch_rate_pos;
-	float _max_pitch_rate_neg;
-	float _max_yaw_rate;
-
 	WheelController _wheel_ctrl;
 
 	void parameters_update();
-	void vehicle_manual_poll(matrix::Quatf R);
+	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_pitch_controller.cpp

@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file fw_pitch_controller.cpp
- * Implementation of a simple pitch P controller.
- */
-
-#include "fw_pitch_controller.h"
-#include <float.h>
-#include <lib/geo/geo.h>
-#include <mathlib/mathlib.h>
-
-float PitchController::control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(pitch_setpoint) &&
-	      PX4_ISFINITE(euler_yaw_rate_setpoint) &&
-	      PX4_ISFINITE(pitch) &&
-	      PX4_ISFINITE(roll))) {
-
-		return _body_rate_setpoint;
-	}
-
-	const float pitch_error = pitch_setpoint - pitch;
-	_euler_rate_setpoint = pitch_error / _tc;
-
-	/* Transform setpoint to body angular rates (jacobian) */
-	const float pitch_body_rate_setpoint_raw = cosf(roll) * _euler_rate_setpoint +
-			cosf(pitch) * sinf(roll) * euler_yaw_rate_setpoint;
-
-	_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate_pos);
-
-	return _body_rate_setpoint;
-}

src/modules/fw_att_control/fw_pitch_controller.h

@@ -1,74 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file fw_pitch_controller.h
- * Definition of a simple pitch P controller.
- */
-
-#ifndef FW_PITCH_CONTROLLER_H
-#define FW_PITCH_CONTROLLER_H
-
-class PitchController
-{
-public:
-	PitchController() = default;
-	~PitchController() = default;
-
-	/**
-	 * @brief Calculates both euler and body pitch rate setpoints.
-	 *
-	 * @param pitch_setpoint pitch setpoint [rad]
-	 * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
-	 * @param roll estimated roll [rad]
-	 * @param pitch estimated pitch [rad]
-	 * @return Pitch body rate setpoint [rad/s]
-	 */
-	float control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);
-
-	void set_time_constant(float time_constant) { _tc = time_constant; }
-	void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; }
-	void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; }
-
-	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
-	float get_body_rate_setpoint() { return _body_rate_setpoint; }
-
-private:
-	float _tc{};
-	float _max_rate_pos{};
-	float _max_rate_neg{};
-	float _euler_rate_setpoint{};
-	float _body_rate_setpoint{};
-};
-
-#endif // FW_PITCH_CONTROLLER_H

src/modules/fw_att_control/fw_roll_controller.cpp

@@ -1,64 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file fw_roll_controller.cpp
- * Implementation of a simple roll P controller.
- */
-
-#include "fw_roll_controller.h"
-#include <float.h>
-#include <lib/geo/geo.h>
-#include <mathlib/mathlib.h>
-
-float RollController::control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(roll_setpoint) &&
-	      PX4_ISFINITE(euler_yaw_rate_setpoint) &&
-	      PX4_ISFINITE(pitch) &&
-	      PX4_ISFINITE(roll))) {
-
-		return _body_rate_setpoint;
-	}
-
-	const float roll_error = roll_setpoint - roll;
-	_euler_rate_setpoint = roll_error / _tc;
-
-	/* Transform setpoint to body angular rates (jacobian) */
-	const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(pitch) *
-			euler_yaw_rate_setpoint;
-	_body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
-
-	return _body_rate_setpoint;
-}

src/modules/fw_att_control/fw_roll_controller.h

@@ -1,72 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file fw_roll_controller.h
- * Definition of a simple roll P controller.
- */
-
-#ifndef FW_ROLL_CONTROLLER_H
-#define FW_ROLL_CONTROLLER_H
-
-class RollController
-{
-public:
-	RollController() = default;
-	~RollController() = default;
-
-	/**
-	 * @brief Calculates both euler and body roll rate setpoints.
-	 *
-	 * @param roll_setpoint roll setpoint [rad]
-	 * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
-	 * @param roll estimated roll [rad]
-	 * @param pitch estimated pitch [rad]
-	 * @return Roll body rate setpoint [rad/s]
-	 */
-	float control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);
-
-	void set_time_constant(float time_constant) { _tc = time_constant; }
-	void set_max_rate(float max_rate) { _max_rate = max_rate; }
-
-	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
-	float get_body_rate_setpoint() { return _body_rate_setpoint; }
-
-private:
-	float _tc{};
-	float _max_rate{};
-	float _euler_rate_setpoint{};
-	float _body_rate_setpoint{};
-};
-
-#endif // FW_ROLL_CONTROLLER_H

src/modules/fw_att_control/fw_yaw_controller.cpp

@@ -1,98 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file fw_yaw_controller.cpp
- * Implementation of a simple coordinated turn yaw controller.
- */
-
-#include "fw_yaw_controller.h"
-#include <float.h>
-#include <lib/geo/geo.h>
-#include <mathlib/mathlib.h>
-
-float YawController::control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
-				 float airspeed)
-{
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(roll_setpoint) &&
-	      PX4_ISFINITE(roll) &&
-	      PX4_ISFINITE(pitch) &&
-	      PX4_ISFINITE(euler_pitch_rate_setpoint) &&
-	      PX4_ISFINITE(airspeed))) {
-
-		return _body_rate_setpoint;
-	}
-
-	float constrained_roll;
-	bool inverted = false;
-
-	/* roll is used as feedforward term and inverted flight needs to be considered */
-	if (fabsf(roll) < math::radians(90.f)) {
-		/* not inverted, but numerically still potentially close to infinity */
-		constrained_roll = math::constrain(roll, math::radians(-80.f), math::radians(80.f));
-
-	} else {
-		inverted = true;
-
-		// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
-		//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
-		if (roll > 0.f) {
-			/* right hemisphere */
-			constrained_roll = math::constrain(roll, math::radians(100.f), math::radians(180.f));
-
-		} else {
-			/* left hemisphere */
-			constrained_roll = math::constrain(roll, math::radians(-180.f), math::radians(-100.f));
-		}
-	}
-
-	constrained_roll = math::constrain(constrained_roll, -fabsf(roll_setpoint), fabsf(roll_setpoint));
-
-
-	if (!inverted) {
-		/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
-		_euler_rate_setpoint = tanf(constrained_roll) * cosf(pitch) * CONSTANTS_ONE_G / airspeed;
-
-		/* Transform setpoint to body angular rates (jacobian) */
-		const float yaw_body_rate_setpoint_raw = -sinf(roll) * euler_pitch_rate_setpoint +
-				cosf(roll) * cosf(pitch) * _euler_rate_setpoint;
-		_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
-	}
-
-	if (!PX4_ISFINITE(_body_rate_setpoint)) {
-		_body_rate_setpoint = 0.f;
-	}
-
-	return _body_rate_setpoint;
-}

src/modules/fw_att_control/fw_yaw_controller.h

@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file fw_yaw_controller.h
- * Definition of a simple coordinated turn controller.
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
- */
-
-#ifndef FW_YAW_CONTROLLER_H
-#define FW_YAW_CONTROLLER_H
-
-class YawController
-{
-public:
-	YawController() = default;
-	~YawController() = default;
-
-	/**
-	 * @brief Calculates both euler and body yaw rate setpoints for coordinated turn based on current attitude and airspeed
-	 *
-	 * @param roll_setpoint roll setpoint [rad]
-	 * @param euler_pitch_rate_setpoint euler pitch rate setpoint [rad/s]
-	 * @param roll estimated roll [rad]
-	 * @param pitch estimated pitch [rad]
-	 * @param airspeed airspeed [m/s]
-	 * @return Roll body rate setpoint [rad/s]
-	 */
-	float control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
-			  float airspeed);
-
-	void set_max_rate(float max_rate) { _max_rate = max_rate; }
-
-	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
-	float get_body_rate_setpoint() { return _body_rate_setpoint; }
-
-private:
-	float _max_rate{};
-	float _euler_rate_setpoint{};
-	float _body_rate_setpoint{};
-};
-
-#endif // FW_YAW_CONTROLLER_H

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -298,7 +298,9 @@ void FwLateralLongitudinalControl::Run()
 			// additional is_finite checks that should not be necessary, but are kept for safety
 			float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
 			float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
-			const float yaw_body = _yaw; // yaw is not controlled in fixed wing, need to set it though for quaternion generation
+
+			float yaw_body = _yaw;
+
 			const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
 
 			if (_control_mode_sub.get().flag_control_manual_enabled) {

src/modules/logger/logged_topics.cpp

@@ -148,7 +148,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_local_position_setpoint", 100);
 	add_topic("vehicle_magnetometer", 200);
 	add_topic("vehicle_rates_setpoint", 20);
-	add_topic("vehicle_euler_rates_setpoint", 20);
 	add_topic("vehicle_roi", 1000);
 	add_topic("vehicle_status");
 	add_topic("vtx");