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

helper_functions add wrap_2pi

Browse Source
This commit is contained in:
Daniel Agar
2018-06-09 16:58:25 -04:00
committed by Lorenz Meier
parent abc8f82d49
commit 03a3e3ad46
2 changed files with 53 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/helper_functions.hpp
+45 -4
View File
@@ -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)) {
x -= Type(2.0 * M_PI);
int c = 0;
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)) {
x += Type(2.0 * M_PI);
c = 0;
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
View File
@@ -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();