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 #include <stdint.h>
  21 #include <stdbool.h>
  22
  23 #ifndef sq
  24 #define sq(x) ((x)*(x))
  25 #endif
  26
  27 // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
  28 #define FAST_MATH             // order 9 approximation
  29 //#define VERY_FAST_MATH      // order 7 approximation
  30
  31 // Use floating point M_PI instead explicitly.
  32 #define M_PIf       3.14159265358979323846f
  33 #define M_LN2f      0.69314718055994530942f
  34 #define M_Ef        2.71828182845904523536f
  35
  36 #define RAD    (M_PIf / 180.0f)
  37
  38 #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
  39 #define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100)
  40
  41 #define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10)
  42 #define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)
  43
  44 #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
  45 #define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
  46
  47 #define DEGREES_PER_DEKADEGREE 10
  48 #define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE)
  49 #define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE)
  50
  51 #define DEGREES_TO_RADIANS(angle) ((angle) * RAD)
  52 #define RADIANS_TO_DEGREES(angle) ((angle) / RAD)
  53 #define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD)
  54 #define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD)
  55
  56 #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
  57 #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
  58
  59 #define CENTIMETERS_TO_CENTIFEET(cm)            (cm * (328 / 100.0))
  60 #define CENTIMETERS_TO_FEET(cm)                 (cm * (328 / 10000.0))
  61 #define CENTIMETERS_TO_METERS(cm)               (cm / 100)
  62
  63 #define METERS_TO_CENTIMETERS(m)                (m * 100)
  64
  65 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
  66 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
  67     ( __extension__ ({                                      \
  68             __typeof__(lexpr) lvar = (lexpr);               \
  69             __typeof__(rexpr) rvar = (rexpr);               \
  70             lvar binoper rvar ? lvar : rvar;                \
  71         }))
  72 #define _CHOOSE_VAR2(prefix, unique) prefix##unique
  73 #define _CHOOSE_VAR(prefix, unique) _CHOOSE_VAR2(prefix, unique)
  74 #define _CHOOSE(binoper, lexpr, rexpr)                   \
  75     _CHOOSE2(                                            \
  76         binoper,                                         \
  77         lexpr, _CHOOSE_VAR(_left, __COUNTER__),          \
  78         rexpr, _CHOOSE_VAR(_right, __COUNTER__)          \
  79         )
  80 #define MIN(a, b) _CHOOSE(<, a, b)
  81 #define MAX(a, b) _CHOOSE(>, a, b)
  82
  83 #define _ABS_II(x, var)                           \
  84     ( __extension__ ({                            \
  85         __typeof__(x) var = (x);                  \
  86         var < 0 ? -var : var;                     \
  87     }))
  88 #define _ABS_I(x, var) _ABS_II(x, var)
  89 #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
  90
  91 // Floating point Euler angles.
  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 stdev_s
 104 {
 105     float m_oldM, m_newM, m_oldS, m_newS;
 106     int m_n;
 107 } stdev_t;
 108
 109 typedef struct filterWithBufferSample_s {
 110     float value;
 111     uint32_t timestamp;
 112 } filterWithBufferSample_t;
 113
 114 typedef struct filterWithBufferState_s {
 115     uint16_t filter_size;
 116     uint16_t sample_index;
 117     filterWithBufferSample_t * samples;
 118 } filterWithBufferState_t;
 119
 120 typedef struct {
 121     float XtY[4];
 122     float XtX[4][4];
 123 } sensorCalibrationState_t;
 124
 125 void sensorCalibrationResetState(sensorCalibrationState_t * state);
 126 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
 127 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target);
 128 bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
 129 bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
 130
 131 int gcd(int num, int denom);
 132 int32_t applyDeadband(int32_t value, int32_t deadband);
 133 float fapplyDeadbandf(float value, float deadband);
 134
 135 int constrain(int amt, int low, int high);
 136 float constrainf(float amt, float low, float high);
 137
 138 void devClear(stdev_t *dev);
 139 void devPush(stdev_t *dev, float x);
 140 float devVariance(stdev_t *dev);
 141 float devStandardDeviation(stdev_t *dev);
 142 float degreesToRadians(int16_t degrees);
 143
 144 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
 145 float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
 146
 147 int32_t wrap_18000(int32_t angle);
 148 int32_t wrap_36000(int32_t angle);
 149
 150 int32_t quickMedianFilter3(int32_t * v);
 151 int32_t quickMedianFilter5(int32_t * v);
 152 int32_t quickMedianFilter7(int32_t * v);
 153 int32_t quickMedianFilter9(int32_t * v);
 154
 155 int16_t quickMedianFilter3_16(int16_t * v);
 156 int16_t quickMedianFilter5_16(int16_t * v);
 157
 158 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
 159 float sin_approx(float x);
 160 float cos_approx(float x);
 161 float atan2_approx(float y, float x);
 162 float acos_approx(float x);
 163 #define tan_approx(x)       (sin_approx(x) / cos_approx(x))
 164 #define asin_approx(x)      (M_PIf / 2 - acos_approx(x))
 165 #else
 166 #define asin_approx(x)      asinf(x)
 167 #define sin_approx(x)       sinf(x)
 168 #define cos_approx(x)       cosf(x)
 169 #define atan2_approx(y,x)   atan2f(y,x)
 170 #define acos_approx(x)      acosf(x)
 171 #define tan_approx(x)       tanf(x)
 172 #endif
 173
 174 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 175
 176 float bellCurve(const float x, const float curveWidth);