From 0125ae26ac8eaa2ae823b942f19df26335fbc14a Mon Sep 17 00:00:00 2001
From: Josh Henderson <hendjoshsr71@gmail.com>
Date: Fri, 20 Aug 2021 01:57:41 -0400
Subject: [PATCH] AP_Math: enable SITL unit testing

---
 libraries/AP_Math/tests/test_math.cpp        | 15 ++++++++++++++-
 libraries/AP_Math/tests/test_math_double.cpp |  5 +++++
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp
index 15981f1248..bdd4e2ec4f 100644
--- a/libraries/AP_Math/tests/test_math.cpp
+++ b/libraries/AP_Math/tests/test_math.cpp
@@ -81,8 +81,15 @@ TEST(VectorTest, Rotations)
     TEST_ROTATION(ROTATION_ROLL_45, 1, 0, SQRT_2);
     TEST_ROTATION(ROTATION_ROLL_315, 1, SQRT_2, 0);
     EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
-    TEST_ROTATION(ROTATION_CUSTOM, 1, 1, 1);  // TODO look at internal error ?
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+    TEST_ROTATION(ROTATION_CUSTOM, 1, 1, 1);
     TEST_ROTATION(ROTATION_MAX, 1, 1, 1);
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    Vector3F v {1, 1, 1};
+    EXPECT_EXIT(v.rotate(ROTATION_CUSTOM), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::flow_of_ctrl");
+    EXPECT_EXIT(v.rotate(ROTATION_MAX), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation");
+#endif
 }
 
 TEST(MathTest, IsZero)
@@ -370,8 +377,13 @@ TEST(MathTest, Constrain)
     EXPECT_EQ(19.9, constrain_value(19.8, 19.9, 20.1));
     EXPECT_EQ(19.9f, constrain_value(19.8f, 19.9f, 20.1f));
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
     EXPECT_EQ(1.0f, constrain_float(nanf("0x4152"), 1.0f, 1.0f));
     EXPECT_EQ(1.0f, constrain_value(nanf("0x4152"), 1.0f, 1.0f));
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    EXPECT_EXIT(constrain_float(nanf("0x4152"), 1.0f, 1.0f), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::cnstring_nan");
+    EXPECT_EXIT(constrain_value(nanf("0x4152"), 1.0f, 1.0f), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::cnstring_nan");
+#endif
 }
 
 TEST(MathWrapTest, Angle180)
@@ -660,6 +672,7 @@ TEST(MathTest, FIXEDWINGTURNRATE)
     EXPECT_NEAR(56.187965393066406f, fixedwing_turn_rate(45, 10.0f), accuracy);
 }
 
+AP_GTEST_PANIC()
 AP_GTEST_MAIN()
 
 #pragma GCC diagnostic pop
diff --git a/libraries/AP_Math/tests/test_math_double.cpp b/libraries/AP_Math/tests/test_math_double.cpp
index 194508e5a4..2d8d831d74 100644
--- a/libraries/AP_Math/tests/test_math_double.cpp
+++ b/libraries/AP_Math/tests/test_math_double.cpp
@@ -86,7 +86,11 @@ TEST(MathTest, ConstrainDouble)
 
     EXPECT_EQ(19.9, constrain_value(19.8, 19.9, 20.1));
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
     EXPECT_EQ(1.0, constrain_value(nan("0x4152"), 1.0, 1.0));
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    EXPECT_EXIT(constrain_value(nan("0x4152"), 1.0, 1.0), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::cnstring_nan");
+#endif
 }
 
 TEST(MathWrapTest, Angle180Double)
@@ -232,6 +236,7 @@ TEST(MathTest, NormDouble)
     EXPECT_DOUBLE_EQ(norm_6, 13.0);
 }
 
+AP_GTEST_PANIC()
 AP_GTEST_MAIN()
 
 #pragma GCC diagnostic pop