flight/pios/inc/pios_math.h

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       pios_math.h
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      Header for math functions/macro definitions
   7  *
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 #ifndef PIOS_MATH_H
  28 #define PIOS_MATH_H
  29 // Generic float math constants
  30 #define M_E_F        2.71828182845904523536028747135f      /* e */
  31 #define M_LOG2E_F    1.44269504088896340735992468100f      /* log_2 (e) */
  32 #define M_LOG10E_F   0.43429448190325182765112891892f      /* log_10 (e) */
  33 #define M_SQRT2_F    1.41421356237309504880168872421f      /* sqrt(2) */
  34 #define M_SQRT1_2_F  0.70710678118654752440084436210f      /* sqrt(1/2) */
  35 #define M_SQRT3_F    1.73205080756887729352744634151f      /* sqrt(3) */
  36 #define M_PI_F       3.14159265358979323846264338328f      /* pi */
  37 #define M_PI_2_F     1.57079632679489661923132169164f      /* pi/2 */
  38 #define M_PI_4_F     0.78539816339744830961566084582f      /* pi/4 */
  39 #define M_SQRTPI_F   1.77245385090551602729816748334f      /* sqrt(pi) */
  40 #define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */
  41 #define M_1_PI_F     0.31830988618379067153776752675f      /* 1/pi */
  42 #define M_2_PI_F     0.63661977236758134307553505349f      /* 2/pi */
  43 #define M_2PI_F      6.28318530717958647692528676656f      /* 2pi  */
  44 #define M_LN10_F     2.30258509299404568401799145468f      /* ln(10) */
  45 #define M_LN2_F      0.69314718055994530941723212146f      /* ln(2) */
  46 #define M_LNPI_F     1.14472988584940017414342735135f      /* ln(pi) */
  47 #define M_EULER_F    0.57721566490153286060651209008f      /* Euler constant */
  48
  49 #define M_E_D        2.71828182845904523536028747135d      /* e */
  50 #define M_LOG2E_D    1.44269504088896340735992468100d      /* log_2 (e) */
  51 #define M_LOG10E_D   0.43429448190325182765112891892d      /* log_10 (e) */
  52 #define M_SQRT2_D    1.41421356237309504880168872421d      /* sqrt(2) */
  53 #define M_SQRT1_2_D  0.70710678118654752440084436210d      /* sqrt(1/2) */
  54 #define M_SQRT3_D    1.73205080756887729352744634151d      /* sqrt(3) */
  55 #define M_PI_D       3.14159265358979323846264338328d      /* pi */
  56 #define M_PI_2_D     1.57079632679489661923132169164d      /* pi/2 */
  57 #define M_PI_4_D     0.78539816339744830961566084582d      /* pi/4 */
  58 #define M_2PI_D      6.28318530717958647692528676656d      /* 2pi  */
  59 #define M_SQRTPI_D   1.77245385090551602729816748334d      /* sqrt(pi) */
  60 #define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */
  61 #define M_1_PI_D     0.31830988618379067153776752675d      /* 1/pi */
  62 #define M_2_PI_D     0.63661977236758134307553505349d      /* 2/pi */
  63 #define M_LN10_D     2.30258509299404568401799145468d      /* ln(10) */
  64 #define M_LN2_D      0.69314718055994530941723212146d      /* ln(2) */
  65 #define M_LNPI_D     1.14472988584940017414342735135d      /* ln(pi) */
  66 #define M_EULER_D    0.57721566490153286060651209008d      /* Euler constant */
  67
  68 // Conversion macro
  69 #define RAD2DEG(rad)            ((rad) * (180.0f / M_PI_F))
  70 #define DEG2RAD(deg)            ((deg) * (M_PI_F / 180.0f))
  71
  72 #define RAD2DEG_D(rad)          ((rad) * (180.0d / M_PI_D))
  73 #define DEG2RAD_D(deg)          ((deg) * (M_PI_D / 180.0d))
  74
  75 // helper macros for LPFs
  76 #define LPF_ALPHA(dt, fc)       (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
  77
  78 // Useful math macros
  79 #define MAX(a, b)               ((a) > (b) ? (a) : (b))
  80 #define MIN(a, b)               ((a) < (b) ? (a) : (b))
  81
  82 #define IS_REAL(f)              (isfinite(f))
  83
  84 // Bitfield access
  85 #define IS_SET(field, mask)     (((field) & (mask)) == (mask))
  86 #define SET_MASK(field, mask)   (field) |= (mask)
  87 #define UNSET_MASK(field, mask) (field) &= ~(mask)
  88
  89 #endif // PIOS_MATH_H