From a42e7ebb2a3333d0cbddf0495dfbfecf7b12387d Mon Sep 17 00:00:00 2001 From: ttechnick Date: Wed, 11 Mar 2026 15:58:58 +0100 Subject: [PATCH] feat(fw_att_control): add yaw rotation unit test --- src/modules/fw_att_control/CMakeLists.txt | 2 + .../FixedwingAttitudeControlTest.cpp | 126 ++++++++++++++++++ 2 files changed, 128 insertions(+) create mode 100644 src/modules/fw_att_control/FixedwingAttitudeControlTest.cpp diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index 40588a3156..749519f9b9 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -42,3 +42,5 @@ px4_add_module( DEPENDS px4_work_queue ) + +px4_add_unit_gtest(SRC FixedwingAttitudeControlTest.cpp) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControlTest.cpp b/src/modules/fw_att_control/FixedwingAttitudeControlTest.cpp new file mode 100644 index 0000000000..75c4f56d73 --- /dev/null +++ b/src/modules/fw_att_control/FixedwingAttitudeControlTest.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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. + * + ****************************************************************************/ + +#include +#include + +using namespace matrix; + +/** + * Computes the body rate setpoint yaw component (before turn coordination) + * using the quaternion-based attitude controller logic from FixedwingAttitudeControl. + * + * @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) +{ + // 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); +} + +TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_Identity) +{ + // When current and setpoint are both identity, yaw rate should be zero + const Quatf q_current; + const Quatf q_sp; + + const float yaw_rate = computeYawRateSetpointBeforeTurnCoord(q_current, q_sp, 1.0f); + + EXPECT_NEAR(yaw_rate, 0.f, 1e-5f); +} + +TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_PureYawError) +{ + // Pure yaw error should result in zero yaw rate setpoint (yaw is compensated out) + 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); + + EXPECT_NEAR(yaw_rate, 0.f, 1e-5f); +} + +TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_RollError) +{ + // Roll error with no pitch/yaw error + 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); + + EXPECT_NEAR(yaw_rate, 0.f, 1e-5f); +} + +TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_PitchError) +{ + // Pitch error with no roll/yaw error + 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); + + EXPECT_NEAR(yaw_rate, 0.f, 1e-5f); +} + +TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_CombinedError) +{ + // Combined roll, pitch, and yaw error + 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); + + EXPECT_NEAR(yaw_rate, 0.f, 1e-4f); +} + +TEST(FixedwingAttitudeControlTest, YawRateSetpointZeroBeforeTurnCoord_BankedTurn) +{ + // Simulating a banked turn scenario - current has roll, setpoint has roll + yaw + 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); + + EXPECT_NEAR(yaw_rate, 0.f, 1e-4f); +}