helper_functions add wrap_2pi

matrix/helper_functions.hpp

@@ -6,6 +6,8 @@
 #include <px4_defines.h>
 #endif
 
+#include <limits>
+
 namespace matrix
 {
 
@@ -27,18 +29,57 @@ Type wrap_pi(Type x)
         return x;
     }
 
-    while (x >= Type(M_PI)) {
+    int c = 0;
-        x -= Type(2.0 * M_PI);
 
+    while (x >= Type(M_PI)) {
+        x -= Type(2 * M_PI);
+
+        if (c++ > 100) {
+            return std::numeric_limits<Type>::quiet_NaN();
+        }
     }
 
-    while (x < Type(-M_PI)) {
+    c = 0;
-        x += Type(2.0 * M_PI);
 
+    while (x < Type(-M_PI)) {
+        x += Type(2 * M_PI);
+
+        if (c++ > 100) {
+            return std::numeric_limits<Type>::quiet_NaN();
+        }
     }
 
     return x;
 }
 
+template<typename Type>
+Type wrap_2pi(Type x)
+{
+    if (!is_finite(x)) {
+        return x;
+    }
+
+    int c = 0;
+
+    while (x >= Type(2 * M_PI)) {
+        x -= Type(2 * M_PI);
+
+        if (c++ > 100) {
+            return std::numeric_limits<Type>::quiet_NaN();
+        }
+    }
+
+    c = 0;
+
+    while (x < Type(0)) {
+        x += Type(2 * M_PI);
+
+        if (c++ > 100) {
+            return std::numeric_limits<Type>::quiet_NaN();
+        }
+    }
+
+    return x;
+}
 
 }

test/helper.cpp

@@ -8,8 +8,16 @@ int main()
     TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < 1e-5);
     TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < 1e-5);
     TEST(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
+    TEST(!is_finite(wrap_pi(1000.0f)));
+    TEST(!is_finite(wrap_pi(-1000.0f)));
     wrap_pi(NAN);
 
+    TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < 1e-5);
+    TEST(fabs(wrap_2pi(3.0) - (3.0)) < 1e-3);
+    TEST(!is_finite(wrap_2pi(1000.0f)));
+    TEST(!is_finite(wrap_2pi(-1000.0f)));
+    wrap_2pi(NAN);
+
     Vector3f a(1, 2, 3);
     Vector3f b(4, 5, 6);
     a.T().print();