Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :   allow for builds with either single or double precision EKF
       5             :  */
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <float.h>
       9             : 
      10             : /*
      11             :   capital F is used to denote the chosen float type (float or double)
      12             :  */
      13             : 
      14             : #if HAL_WITH_EKF_DOUBLE
      15             : typedef double ftype;
      16             : #define sinF(x) sin(x)
      17             : #define acosF(x) acos(x)
      18             : #define asinF(x) asin(x)
      19             : #define cosF(x) cos(x)
      20             : #define tanF(x) tan(x)
      21             : #define atanF(x) atan(x)
      22             : #define atan2F(x,y) atan2(x,y)
      23             : #define sqrtF(x) sqrt(x)
      24             : #define fmaxF(x,y) fmax(x,y)
      25             : #define powF(x,y) pow(x,y)
      26             : #define logF(x) log(x)
      27             : #define fabsF(x) fabs(x)
      28             : #define ceilF(x) ceil(x)
      29             : #define fminF(x,y) fmin(x,y)
      30             : #define fmodF(x,y) fmod(x,y)
      31             : #define fabsF(x) fabs(x)
      32             : #define toftype todouble
      33             : #else
      34             : typedef float ftype;
      35             : #define acosF(x) acosf(x)
      36             : #define asinF(x) asinf(x)
      37             : #define sinF(x) sinf(x)
      38             : #define cosF(x) cosf(x)
      39             : #define tanF(x) tanf(x)
      40             : #define atanF(x) atanf(x)
      41             : #define atan2F(y,x) atan2f(y,x)
      42             : #define sqrtF(x) sqrtf(x)
      43             : #define fmaxF(x,y) fmaxf(x,y)
      44             : #define powF(x,y) powf(x,y)
      45             : #define logF(x) logf(x)
      46             : #define fabsF(x) fabsf(x)
      47             : #define ceilF(x) ceilf(x)
      48             : #define fminF(x,y) fminf(x,y)
      49             : #define fmodF(x,y) fmodf(x,y)
      50             : #define fabsF(x) fabsf(x)
      51             : #define toftype tofloat
      52             : #endif
      53             : 
      54             : #if MATH_CHECK_INDEXES
      55             : #define ZERO_FARRAY(a) a.zero()
      56             : #else
      57             : #define ZERO_FARRAY(a) memset(a, 0, sizeof(a))
      58             : #endif
      59             : 
      60             : /*
      61             :  * @brief: Check whether a float is zero
      62             :  */
      63 50611151147 : inline bool is_zero(const float x) {
      64 50611151147 :     return fabsf(x) < FLT_EPSILON;
      65             : }
      66             : 
      67             : /*
      68             :  * @brief: Check whether a double is zero
      69             :  */
      70  1167863296 : inline bool is_zero(const double x) {
      71             : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
      72  1167863296 :   return fabs(x) < FLT_EPSILON;
      73             : #else
      74             :   return fabsf(static_cast<float>(x)) < FLT_EPSILON;
      75             : #endif
      76             : }