PX4 Firmware
PX4 Autopilot Software http://px4.io
helper_functions.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include "math.hpp"
4 
5 #if defined (__PX4_NUTTX) || defined (__PX4_QURT)
6 #include <px4_defines.h>
7 #endif
8 
9 namespace matrix
10 {
11 
12 template<typename Type>
13 bool is_finite(Type x) {
14 #if defined (__PX4_NUTTX)
15  return PX4_ISFINITE(x);
16 #elif defined (__PX4_QURT)
17  return __builtin_isfinite(x);
18 #else
19  return std::isfinite(x);
20 #endif
21 }
22 
23 /**
24  * Compare if two floating point numbers are equal
25  *
26  * NAN is considered equal to NAN and -NAN
27  * INFINITY is considered equal INFINITY but not -INFINITY
28  *
29  * @param x right side of equality check
30  * @param y left side of equality check
31  * @param eps numerical tolerance of the check
32  * @return true if the two values are considered equal, false otherwise
33  */
34 template<typename Type>
35 bool isEqualF(const Type x, const Type y, const Type eps = 1e-4f)
36 {
37  return (matrix::fabs(x - y) <= eps)
38  || (isnan(x) && isnan(y))
39  || (isinf(x) && isinf(y) && isnan(x - y));
40 }
41 
42 /**
43  * Wrap value to stay in range [low, high)
44  *
45  * @param x input possibly outside of the range
46  * @param low lower limit of the allowed range
47  * @param high upper limit of the allowed range
48  * @return wrapped value inside the range
49  */
50 template<typename Type>
51 Type wrap(Type x, Type low, Type high) {
52  // already in range
53  if (low <= x && x < high) {
54  return x;
55  }
56 
57  const Type range = high - low;
58  const Type inv_range = Type(1) / range; // should evaluate at compile time, multiplies below at runtime
59  const Type num_wraps = floor((x - low) * inv_range);
60  return x - range * num_wraps;
61 }
62 
63 /**
64  * Wrap value in range [-π, π)
65  */
66 template<typename Type>
67 Type wrap_pi(Type x)
68 {
69  return wrap(x, Type(-M_PI), Type(M_PI));
70 }
71 
72 /**
73  * Wrap value in range [0, 2π)
74  */
75 template<typename Type>
76 Type wrap_2pi(Type x)
77 {
78  return wrap(x, Type(0), Type(M_TWOPI));
79 }
80 
81 }
Type wrap(Type x, Type low, Type high)
Wrap value to stay in range [low, high)
#define M_TWOPI
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
bool is_finite(Type x)
Type wrap_pi(Type x)
Wrap value in range [-π, π)
#define M_PI
Definition: gps_helper.cpp:38
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Dual< Scalar, N > floor(const Dual< Scalar, N > &a)
Definition: Dual.hpp:210