From f8220a8adfdce876034ef2a78f7b411fb3a09ec0 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 23 Jul 2021 20:45:50 +0100
Subject: [PATCH] AP_Math: tests: test quaternion rotation is the same as
 vector

---
 libraries/AP_Math/tests/test_math.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp
index 6ce570f463..584d33ce3b 100644
--- a/libraries/AP_Math/tests/test_math.cpp
+++ b/libraries/AP_Math/tests/test_math.cpp
@@ -19,12 +19,20 @@ TEST(VectorTest, Rotations)
 #define TEST_ROTATION(rotation, _x, _y, _z) { \
     const float accuracy = 1.0e-6; \
     Vector3f v(1, 1, 1); \
+    Vector3f v2 = v; \
     v.rotate(rotation); \
     Vector3f expected(_x, _y, _z); \
     EXPECT_NEAR(expected.length(), v.length(), accuracy); \
     EXPECT_FLOAT_EQ(expected.x, v.x); \
     EXPECT_FLOAT_EQ(expected.y, v.y); \
     EXPECT_FLOAT_EQ(expected.z, v.z); \
+    Quaternion quat; \
+    quat.from_rotation(rotation); \
+    quat.earth_to_body(v2); \
+    EXPECT_NEAR(expected.length(), v.length(), accuracy); \
+    EXPECT_NEAR(expected.x, v2.x, accuracy); \
+    EXPECT_NEAR(expected.y, v2.y, accuracy); \
+    EXPECT_NEAR(expected.z, v2.z, accuracy); \
     rotation_count++; \
 }