src/main/common/maths.h

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #pragma once
  22
  23 #include <stdint.h>
  24
  25 #ifndef sq
  26 #define sq(x) ((x)*(x))
  27 #endif
  28 #define power3(x) ((x)*(x)*(x))
  29 #define power5(x) ((x)*(x)*(x)*(x)*(x))
  30
  31 // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
  32 #define FAST_MATH             // order 9 approximation
  33 #define VERY_FAST_MATH        // order 7 approximation
  34
  35 // Use floating point M_PI instead explicitly.
  36 #define M_PIf       3.14159265358979323846f
  37 #define M_EULERf    2.71828182845904523536f
  38
  39 #define RAD    (M_PIf / 180.0f)
  40 #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
  41 #define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
  42 #define DECIDEGREES_TO_RADIANS(angle) ((angle) / 10.0f * 0.0174532925f)
  43 #define DEGREES_TO_RADIANS(angle) ((angle) * RAD)
  44 #define RADIANS_TO_DEGREES(angle) ((angle) / RAD)
  45
  46
  47 #define CM_S_TO_KM_H(centimetersPerSecond) ((centimetersPerSecond) * 36 / 1000)
  48 #define CM_S_TO_MPH(centimetersPerSecond) ((centimetersPerSecond) * 10000 / 5080 / 88)
  49
  50 #define MIN(a,b) \
  51   __extension__ ({ __typeof__ (a) _a = (a); \
  52   __typeof__ (b) _b = (b); \
  53   _a < _b ? _a : _b; })
  54 #define MAX(a,b) \
  55   __extension__ ({ __typeof__ (a) _a = (a); \
  56   __typeof__ (b) _b = (b); \
  57   _a > _b ? _a : _b; })
  58 #define ABS(x) \
  59   __extension__ ({ __typeof__ (x) _x = (x); \
  60   _x > 0 ? _x : -_x; })
  61 #define SIGN(x) \
  62   __extension__ ({ __typeof__ (x) _x = (x); \
  63   (_x > 0) - (_x < 0); })
  64
  65 #define Q12 (1 << 12)
  66
  67 #define HZ_TO_INTERVAL(x) (1.0f / (x))
  68 #define HZ_TO_INTERVAL_US(x) (1000000 / (x))
  69
  70 typedef int32_t fix12_t;
  71
  72 typedef struct stdev_s
  73 {
  74     float m_oldM, m_newM, m_oldS, m_newS;
  75     int m_n;
  76 } stdev_t;
  77
  78 // Floating point 3 vector.
  79 typedef struct fp_vector {
  80     float X;
  81     float Y;
  82     float Z;
  83 } t_fp_vector_def;
  84
  85 typedef union u_fp_vector {
  86     float A[3];
  87     t_fp_vector_def V;
  88 } t_fp_vector;
  89
  90 // Floating point Euler angles.
  91 // Be carefull, could be either of degrees or radians.
  92 typedef struct fp_angles {
  93     float roll;
  94     float pitch;
  95     float yaw;
  96 } fp_angles_def;
  97
  98 typedef union {
  99     float raw[3];
 100     fp_angles_def angles;
 101 } fp_angles_t;
 102
 103 typedef struct fp_rotationMatrix_s {
 104     float m[3][3];              // matrix
 105 } fp_rotationMatrix_t;
 106
 107 int gcd(int num, int denom);
 108 int32_t applyDeadband(int32_t value, int32_t deadband);
 109 float fapplyDeadband(float value, float deadband);
 110
 111 void devClear(stdev_t *dev);
 112 void devPush(stdev_t *dev, float x);
 113 float devVariance(stdev_t *dev);
 114 float devStandardDeviation(stdev_t *dev);
 115 float degreesToRadians(int16_t degrees);
 116
 117 int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
 118 float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo);
 119
 120 void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation);
 121 void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix);
 122
 123 int32_t quickMedianFilter3(int32_t * v);
 124 int32_t quickMedianFilter5(int32_t * v);
 125 int32_t quickMedianFilter7(int32_t * v);
 126 int32_t quickMedianFilter9(int32_t * v);
 127
 128 float quickMedianFilter3f(float * v);
 129 float quickMedianFilter5f(float * v);
 130 float quickMedianFilter7f(float * v);
 131 float quickMedianFilter9f(float * v);
 132
 133 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
 134 float sin_approx(float x);
 135 float cos_approx(float x);
 136 float atan2_approx(float y, float x);
 137 float acos_approx(float x);
 138 #define tan_approx(x)       (sin_approx(x) / cos_approx(x))
 139 float exp_approx(float val);
 140 float log_approx(float val);
 141 float pow_approx(float a, float b);
 142 #else
 143 #define sin_approx(x)       sinf(x)
 144 #define cos_approx(x)       cosf(x)
 145 #define atan2_approx(y,x)   atan2f(y,x)
 146 #define acos_approx(x)      acosf(x)
 147 #define tan_approx(x)       tanf(x)
 148 #define exp_approx(x)       expf(x)
 149 #define log_approx(x)       logf(x)
 150 #define pow_approx(a, b)    powf(b, a)
 151 #endif
 152
 153 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 154
 155 int16_t qPercent(fix12_t q);
 156 int16_t qMultiply(fix12_t q, int16_t input);
 157 fix12_t qConstruct(int16_t num, int16_t den);
 158
 159 static inline int constrain(int amt, int low, int high)
 160 {
 161     if (amt < low)
 162         return low;
 163     else if (amt > high)
 164         return high;
 165     else
 166         return amt;
 167 }
 168
 169 static inline float constrainf(float amt, float low, float high)
 170 {
 171     if (amt < low)
 172         return low;
 173     else if (amt > high)
 174         return high;
 175     else
 176         return amt;
 177 }