fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 05:33:33 +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-10 10:56:45 +02: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;
}
}