From 419b70e4b51606cf135882d07a75311309cfd1aa Mon Sep 17 00:00:00 2001
From: kamilritz <kritz@ethz.ch>
Date: Mon, 17 Aug 2020 20:00:13 +0200
Subject: [PATCH] Add tests for shouldUse321RotationSequence

---
 test/test_EKF_utils.cpp | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/test/test_EKF_utils.cpp b/test/test_EKF_utils.cpp
index 03ed0ba22c..81df57446b 100644
--- a/test/test_EKF_utils.cpp
+++ b/test/test_EKF_utils.cpp
@@ -40,6 +40,7 @@
 #include <gtest/gtest.h>
 #include <cmath>
 #include <vector>
+#include <mathlib/mathlib.h>
 
 #include "EKF/utils.hpp"
 
@@ -68,3 +69,31 @@ TEST(euler312YawTest, fromQuaternion)
 	const matrix::Eulerf euler2(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));
+}