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) * 0.01f)
  40
  41 #define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f)
  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.0f)
  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) * 0.01f) * RAD)
  58
  59 #define MILLIMETERS_TO_CENTIMETERS(mm)  (mm / 10.0f)
  60
  61 #define CENTIMETERS_TO_CENTIFEET(cm)    (cm / 0.3048f)
  62 #define CENTIMETERS_TO_FEET(cm)         (cm / 30.48f)
  63 #define CENTIMETERS_TO_METERS(cm)       (cm * 0.01f)
  64
  65 #define METERS_TO_CENTIMETERS(m)        (m * 100)
  66 #define METERS_TO_KILOMETERS(m)         (m / 1000.0f)
  67 #define METERS_TO_MILES(m)              (m / 1609.344f)
  68 #define METERS_TO_NAUTICALMILES(m)      (m / 1852.00f)
  69
  70 #define MWH_TO_WH(mWh)                  (mWh / 1000.0f)
  71
  72 #define CMSEC_TO_CENTIMPH(cms)          (cms * 2.2369363f)
  73 #define CMSEC_TO_CENTIKPH(cms)          (cms * 3.6f)
  74 #define CMSEC_TO_CENTIKNOTS(cms)        (cms * 1.943845f)
  75
  76 #define CMSEC_TO_MPH(cms)          (CMSEC_TO_CENTIMPH(cms) / 100.0f)
  77 #define CMSEC_TO_KPH(cms)          (CMSEC_TO_CENTIKPH(cms) / 100.0f)
  78 #define CMSEC_TO_KNOTS(cms)        (CMSEC_TO_CENTIKNOTS(cms) / 100.0f)
  79
  80 #define C_TO_KELVIN(temp) (temp + 273.15f)
  81
  82 // Standard Sea Level values
  83 // Ref:https://en.wikipedia.org/wiki/Standard_sea_level
  84 #define SSL_AIR_DENSITY         1.225f // kg/m^3
  85 #define SSL_AIR_PRESSURE        101325.01576f // Pascal
  86 #define SSL_AIR_TEMPERATURE     288.15f // K
  87
  88 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
  89 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \
  90     ( __extension__ ({                              \
  91         __typeof__(lexpr) lvar = (lexpr);           \
  92         __typeof__(rexpr) rvar = (rexpr);           \
  93         lvar binoper rvar ? lvar : rvar;            \
  94     }))
  95 #define _CHOOSE_VAR2(prefix, unique) prefix##unique
  96 #define _CHOOSE_VAR(prefix, unique) _CHOOSE_VAR2(prefix, unique)
  97 #define _CHOOSE(binoper, lexpr, rexpr)          \
  98     _CHOOSE2(                                   \
  99         binoper,                                \
 100         lexpr, _CHOOSE_VAR(_left, __COUNTER__), \
 101         rexpr, _CHOOSE_VAR(_right, __COUNTER__) \
 102         )
 103 #define MIN(a, b) _CHOOSE(<, a, b)
 104 #define MAX(a, b) _CHOOSE(>, a, b)
 105 #define SIGN(a) ((a >= 0) ? 1 : -1)
 106
 107 #define _ABS_II(x, var)             \
 108     ( __extension__ ({              \
 109         __typeof__(x) var = (x);    \
 110         var < 0 ? -var : var;       \
 111     }))
 112 #define _ABS_I(x, var) _ABS_II(x, var)
 113 #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
 114
 115 #define power3(x) ((x)*(x)*(x))
 116
 117 // Floating point Euler angles.
 118 typedef struct fp_angles {
 119     float roll;
 120     float pitch;
 121     float yaw;
 122 } fp_angles_def;
 123
 124 typedef union {
 125     float raw[3];
 126     fp_angles_def angles;
 127 } fp_angles_t;
 128
 129 typedef struct stdev_s {
 130     float m_oldM, m_newM, m_oldS, m_newS;
 131     int m_n;
 132 } stdev_t;
 133
 134 typedef struct filterWithBufferSample_s {
 135     float value;
 136     uint32_t timestamp;
 137 } filterWithBufferSample_t;
 138
 139 typedef struct filterWithBufferState_s {
 140     uint16_t filter_size;
 141     uint16_t sample_index;
 142     filterWithBufferSample_t * samples;
 143 } filterWithBufferState_t;
 144
 145 typedef struct {
 146     float XtY[4];
 147     float XtX[4][4];
 148 } sensorCalibrationState_t;
 149
 150 void sensorCalibrationResetState(sensorCalibrationState_t * state);
 151 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3]);
 152 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target);
 153 bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
 154 bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
 155
 156 int gcd(int num, int denom);
 157 int32_t applyDeadband(int32_t value, int32_t deadband);
 158 int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max);
 159
 160 int32_t constrain(int32_t amt, int32_t low, int32_t high);
 161 float constrainf(float amt, float low, float high);
 162
 163 void devClear(stdev_t *dev);
 164 void devPush(stdev_t *dev, float x);
 165 float devVariance(stdev_t *dev);
 166 float devStandardDeviation(stdev_t *dev);
 167 float degreesToRadians(int16_t degrees);
 168
 169 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
 170 float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
 171
 172 int32_t wrap_18000(int32_t angle);
 173 int16_t wrap_180(int16_t angle);
 174 int32_t wrap_36000(int32_t angle);
 175
 176 int32_t quickMedianFilter3(int32_t * v);
 177 int32_t quickMedianFilter5(int32_t * v);
 178 int32_t quickMedianFilter7(int32_t * v);
 179 int32_t quickMedianFilter9(int32_t * v);
 180
 181 int16_t quickMedianFilter3_16(int16_t * v);
 182 int16_t quickMedianFilter5_16(int16_t * v);
 183
 184 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
 185 float sin_approx(float x);
 186 float cos_approx(float x);
 187 float atan2_approx(float y, float x);
 188 float acos_approx(float x);
 189 #define tan_approx(x)       (sin_approx(x) / cos_approx(x))
 190 #define asin_approx(x)      (M_PIf / 2 - acos_approx(x))
 191 #else
 192 #define asin_approx(x)      asinf(x)
 193 #define sin_approx(x)       sinf(x)
 194 #define cos_approx(x)       cosf(x)
 195 #define atan2_approx(y,x)   atan2f(y,x)
 196 #define acos_approx(x)      acosf(x)
 197 #define tan_approx(x)       tanf(x)
 198 #endif
 199
 200 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 201
 202 float bellCurve(const float x, const float curveWidth);
 203 float attenuation(const float input, const float width);
 204 float gaussian(const float x, const float mu, const float sigma);
 205 float fast_fsqrtf(const float value);
 206 float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
 207 float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);
 208
 209 /*
 210  * The most significat byte is placed at the lowest address
 211  * in other words, the most significant byte is "first", on even indexes
 212  */
 213 #define int16_val_big_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1]))
 214 /*
 215  * The most significat byte is placed at the highest address
 216  * in other words, the most significant byte is "last", on odd indexes
 217  */
 218 #define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
 219
 220 #ifdef SITL_BUILD
 221 void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
 222 void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize);
 223 void arm_mult_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
 224 #endif