src/main/common/maths.h

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