feat(fw_att_control): add derrivation of formulas

refactor(fw_att_control): move error calc to function

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -297,21 +297,16 @@ void FixedwingAttitudeControl::Run()
 
 				if (q_sp.isAllFinite()) {
 					const Quatf q_current(att.q);
-					// We rotate the setpoint, to prevent yaw errors that we do not have authority to control.
-					const float yaw_offset = -2.f * (q_current(0) * q_sp(3) - q_current(1) * q_sp(2) + q_current(2) * q_sp(1) - q_current(3) * q_sp(0)) /
-								 (q_current(0) * q_sp(0) - q_current(1) * q_sp(1) - q_current(2) * q_sp(2) + q_current(3) * q_sp(3));
-					const Quatf q_yaw_offset = Quatf(1.f, 0.f, 0.f, yaw_offset / 2.f).normalized();
-					const Quatf q_err = (q_current.inversed() * q_yaw_offset * q_sp).canonical(); // See mc_att_control for details
-					const Vector3f q_err_imag = 2.f * q_err.imag();
+					const Vector3f att_err = computeAttitudeError(q_current, q_sp);
 
 					Vector3f body_rates_setpoint;
-					body_rates_setpoint = _proportional_gain.emult(q_err_imag);
+					body_rates_setpoint = _proportional_gain.emult(att_err);
 
 					// Turn coordination
 					const float V = math::max(get_airspeed_constrained(), 0.1f);
 					const float q1 = 2.f * (q_current(0) * q_current(1) + q_current(2) * q_current(3));
 					const float r_tc_ff = CONSTANTS_ONE_G * q1 / V;
-					const float p_tc_ff = 2.f * q1 * r_tc_ff / (1.f - 2.f * q_current(1) * q_current(1) - 2.f * q_current(2) * q_current(2));
+					const float p_tc_ff = q1 * r_tc_ff / (1.f - 2.f * q_current(1) * q_current(1) - 2.f * q_current(2) * q_current(2));
 
 					body_rates_setpoint(1) += p_tc_ff;
 					body_rates_setpoint(2) += r_tc_ff;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -66,11 +66,28 @@
 
 using matrix::Eulerf;
 using matrix::Quatf;
+using matrix::Vector3f;
 
 using uORB::SubscriptionData;
 
 using namespace time_literals;
 
+/**
+ * Computes the attitude error for fixed-wing attitude control.
+ * The yaw error is removed since fixed-wing aircraft have no direct yaw authority.
+ */
+inline Vector3f computeAttitudeError(const Quatf &q_current, const Quatf &q_sp)
+{
+	// Compute yaw offset to cancel yaw error (fixed-wing has no direct yaw authority)
+	// See formula_derrivation.py on how to get these formulas
+	const float yaw_offset = -2.f * (q_current(0) * q_sp(3) - q_current(1) * q_sp(2) + q_current(2) * q_sp(1) - q_current(3) * q_sp(0)) /
+				 (q_current(0) * q_sp(0) - q_current(1) * q_sp(1) - q_current(2) * q_sp(2) + q_current(3) * q_sp(3));
+
+	const Quatf q_yaw_offset = Quatf(1.f, 0.f, 0.f, yaw_offset / 2.f).normalized();
+	const Quatf q_err = (q_current.inversed() * q_yaw_offset * q_sp).canonical();
+	return 2.f * q_err.imag();
+}
+
 class FixedwingAttitudeControl final : public ModuleBase, public ModuleParams,
 	public px4::ScheduledWorkItem
 {

src/modules/fw_att_control/FixedwingAttitudeControlTest.cpp

@@ -32,31 +32,22 @@
  ****************************************************************************/
 
 #include <gtest/gtest.h>
-#include <matrix/matrix/math.hpp>
+#include "FixedwingAttitudeControl.hpp"
 
 using namespace matrix;
 
 /**
  * Computes the body rate setpoint yaw component (before turn coordination)
- * using the quaternion-based attitude controller logic from FixedwingAttitudeControl.
+ * using the shared computeAttitudeError function from FixedwingAttitudeControl.hpp.
  *
  * @param q_current Current attitude quaternion
  * @param q_sp Attitude setpoint quaternion
- * @param yaw_gain Proportional gain for yaw (typically 1.0)
  * @return Yaw rate setpoint before turn coordination feedforward
  */
-static float computeYawRateSetpointBeforeTurnCoord(const Quatf &q_current, const Quatf &q_sp, float yaw_gain)
+static float computeYawRateSetpointBeforeTurnCoord(const Quatf &q_current, const Quatf &q_sp)
 {
-	// Compute yaw offset to cancel yaw error (fixed-wing has no direct yaw authority)
-	const float yaw_offset = -2.f * (q_current(0) * q_sp(3) - q_current(1) * q_sp(2) + q_current(2) * q_sp(1) -
-					 q_current(3) * q_sp(0)) /
-				 (q_current(0) * q_sp(0) - q_current(1) * q_sp(1) - q_current(2) * q_sp(2) + q_current(3) * q_sp(3));
-
-	const Quatf q_yaw_offset = Quatf(1.f, 0.f, 0.f, yaw_offset / 2.f).normalized();
-	const Quatf q_err = (q_current.inversed() * q_yaw_offset * q_sp).canonical();
-	const Vector3f q_err_imag = 2.f * q_err.imag();
-
-	return yaw_gain * q_err_imag(2);
+	const Vector3f att_err = computeAttitudeError(q_current, q_sp);
+	return att_err(2);
 }
 
 TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_Identity)
@@ -65,8 +56,7 @@ TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_Identity)
 	const Quatf q_current;
 	const Quatf q_sp;
 
-	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f);
-
+	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp);
 	EXPECT_NEAR(yaw_rate, 0.f, 1e-5f);
 }
 
@@ -76,7 +66,7 @@ TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_PureYawErr
 	const Quatf q_current;
 	const Quatf q_sp(Eulerf(0.f, 0.f, 0.5f)); // 0.5 rad (~29 deg) yaw offset
 
-	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f);
+	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp);
 
 	EXPECT_NEAR(yaw_rate, 0.f, 1e-5f);
 }
@@ -87,7 +77,7 @@ TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_RollError)
 	const Quatf q_current;
 	const Quatf q_sp(Eulerf(0.3f, 0.f, 0.f)); // 0.3 rad roll
 
-	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f);
+	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp);
 
 	EXPECT_NEAR(yaw_rate, 0.f, 1e-5f);
 }
@@ -98,7 +88,7 @@ TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_PitchError
 	const Quatf q_current;
 	const Quatf q_sp(Eulerf(0.f, 0.2f, 0.f)); // 0.2 rad pitch
 
-	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f);
+	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp);
 
 	EXPECT_NEAR(yaw_rate, 0.f, 1e-5f);
 }
@@ -109,7 +99,7 @@ TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_CombinedEr
 	const Quatf q_current(Eulerf(0.1f, 0.05f, 0.2f));
 	const Quatf q_sp(Eulerf(0.4f, 0.15f, 0.8f));
 
-	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f);
+	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp);
 
 	EXPECT_NEAR(yaw_rate, 0.f, 1e-4f);
 }
@@ -120,7 +110,7 @@ TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_BankedTurn
 	const Quatf q_current(Eulerf(0.5f, 0.f, 0.f)); // 30 deg bank
 	const Quatf q_sp(Eulerf(0.5f, 0.f, 0.3f));     // same bank, different heading
 
-	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f);
+	const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp);
 
 	EXPECT_NEAR(yaw_rate, 0.f, 1e-4f);
 }

src/modules/fw_att_control/formula_derrivation.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python3
+import symforce.symbolic as sf
+
+
+def section(title: str) -> None:
+    print("\n" + "=" * 80)
+    print(title)
+    print("=" * 80)
+
+
+def derive_turn_coordination() -> None:
+    section("Turn coordination")
+
+    # Quaternion attitude of the aircraft.
+    # This represents the rotation from body frame -> earth frame.
+    R = sf.Rot3.symbolic("q")
+
+    # Body angular velocity vector.
+    # This contains the aircraft rotational rates:
+    #   w_b[0] = roll rate (p)
+    #   w_b[1] = pitch rate (q)
+    #   w_b[2] = yaw rate (r)
+    w_b = sf.V3.symbolic("w_b")
+
+    # True airspeed magnitude of the aircraft.
+    v = sf.Symbol("v")
+
+    # Angle of attack (angle between body x-axis and velocity vector).
+    alpha = sf.Symbol("alpha")
+    q_w, q_x, q_y, q_z = sf.symbols("q_w q_x q_y q_z")
+
+    # Velocity vector expressed in the body frame.
+    #
+    # We assume no sideslip, meaning the velocity lies in the body x-z plane.
+    v_b = sf.V3(v * sf.cos(alpha), 0, v * sf.sin(alpha))
+
+    # Gravity magnitude.
+    g_z = sf.Symbol("g")
+    g = sf.V3(0, 0, -g_z)
+
+    # Centrifugal acceleration experienced during a turn.
+    #
+    # a = -ω × v
+    #
+    # This comes from the rotating reference frame of the aircraft.
+    # If the aircraft is turning, the velocity vector produces a
+    # centrifugal acceleration relative to the body frame.
+    a_centrifugal = -w_b.cross(v_b)
+
+    # Total body acceleration required to maintain the turn.
+    #
+    # We must cancel the centrifugal acceleration AND account for gravity.
+    #
+    # Gravity is defined in the earth frame, so we rotate it into the
+    # body frame using R.inverse().
+    a_b = -a_centrifugal + R.inverse() * g
+
+    # Steady coordinated turn assumption:
+    #
+    # Roll rate p = 0 (bank angle is constant)
+    #
+    # This removes transient roll motion from the equations.
+    a_b = a_b.subs({w_b[0]: 0})
+
+    print("a_b =")
+    print(a_b)
+
+    # Solve for the body yaw rate r = w_b[2].
+    #
+    # We use the lateral acceleration component a_b[1].
+    #
+    # In a coordinated turn this must be zero (no lateral slip).
+    # Solving this gives the yaw rate required to sustain the turn.
+    yaw_rate_body = sf.solve(a_b[1], w_b[2])[0]
+
+    print("\nyaw_rate_body =")
+    print(yaw_rate_body)
+
+    # Now compute the centrifugal acceleration expressed in the earth frame.
+    #
+    # This helps derive relationships between pitch rate and yaw rate
+    # that maintain the circular flight path.
+    a_centrifugal_e = R * a_centrifugal
+
+    # For simplicity we assume zero angle of attack.
+    #
+    # This isolates the rotational dynamics of the turn itself.
+    a_centrifugal_e = a_centrifugal_e.subs({alpha: 0})
+
+    print("\na_centrifugal_e =")
+    print(a_centrifugal_e)
+
+    # Solve the vertical earth-frame centrifugal acceleration component
+    # for pitch rate q = w_b[1].
+    #
+    # This determines the pitch rate required to maintain the circular
+    # trajectory while the aircraft is yawing.
+    pitch_rate_body = sf.solve(a_centrifugal_e[2], w_b[1])[0]
+
+    print("\npitch_rate_body (raw) =")
+    print(pitch_rate_body)
+
+    # Substitute the yaw-rate expression into the pitch-rate equation.
+    #
+    # This removes w_b[2] and expresses the pitch rate purely in terms
+    # of quaternion attitude, gravity, airspeed, and angle of attack.
+    pitch_rate_body_subbed = pitch_rate_body.subs({w_b[2]: yaw_rate_body})
+
+    print("\npitch_rate_body_with_yaw_subbed =")
+    print(pitch_rate_body_subbed)
+
+
+def derive_yaw_offset() -> None:
+    section("Yaw offset")
+
+    # Current aircraft attitude quaternion.
+    q = sf.Quaternion.symbolic("q")
+
+    # Desired attitude quaternion setpoint.
+    q_sp = sf.Quaternion.symbolic("q_sp")
+
+    # Small yaw correction we want to solve for.
+    yaw_off = sf.Symbol("yaw_off")
+
+    q_w, q_x, q_y, q_z = sf.symbols("q_w q_x q_y q_z")
+    q_sp_w, q_sp_x, q_sp_y, q_sp_z = sf.symbols("q_sp_w q_sp_x q_sp_y q_sp_z")
+
+    # Quaternion representing a small yaw rotation.
+    #
+    # For small angles:
+    #   q ≈ [0, 0, yaw/2, 1]
+    #
+    # This approximation simplifies the algebra.
+    q_yaw_off = sf.Quaternion(xyz=sf.V3(0.0, 0.0, yaw_off / 2), w=1)
+
+    print("q_yaw_off =")
+    print(q_yaw_off)
+
+    # Compute attitude error quaternion.
+    #
+    # delta_q represents the rotation needed to move
+    # from the current attitude q to the desired attitude q_sp
+    # with an additional yaw offset applied.
+    delta_q = q.conj() * q_yaw_off * q_sp
+
+    print("\ndelta_q.z =")
+    print(delta_q.z)
+
+    # Solve the z component of the error quaternion for yaw_off.
+    #
+    # Setting delta_q.z = 0 corresponds to removing yaw error.
+    yaw_off_solution = sf.solve(delta_q.z, yaw_off)[0]
+
+    print("\nyaw_off =")
+    print(yaw_off_solution)
+
+
+if __name__ == "__main__":
+    derive_turn_coordination()
+    derive_yaw_offset()