fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 20:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add tests for shouldUse321RotationSequence

Browse Source
This commit is contained in:
kamilritz
2020-08-17 20:00:13 +02:00
committed by Paul Riseborough
parent 4fb4e0ca01
commit 419b70e4b5
1 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_utils.cpp
+29
View File
@@ -40,6 +40,7 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <cmath> #include <cmath>
#include <vector> #include <vector>
#include <mathlib/mathlib.h>
#include "EKF/utils.hpp" #include "EKF/utils.hpp"
@@ -68,3 +69,31 @@ TEST(euler312YawTest, fromQuaternion)
const matrix::Eulerf euler2(q2); const matrix::Eulerf euler2(q2);
EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2)); EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2));
} }
TEST(shouldUse321RotationSequenceTest, pitch90)
{
matrix::Eulerf euler(0.f, math::radians(90), 0.f);
matrix::Dcmf R(euler);
EXPECT_FALSE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, roll90)
{
matrix::Eulerf euler(math::radians(90.f), 0.f, 0.f);
matrix::Dcmf R(euler);
EXPECT_TRUE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, moreRollThanPitch)
{
matrix::Eulerf euler(math::radians(45.f), math::radians(30.f), 0.f);
matrix::Dcmf R(euler);
EXPECT_TRUE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, morePitchThanRoll)
{
matrix::Eulerf euler(math::radians(30.f), math::radians(45.f), 0.f);
matrix::Dcmf R(euler);
EXPECT_FALSE(shouldUse321RotationSequence(R));
}