src/main/common/maths.c

   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 #include <stdint.h>
  19 #include <math.h>
  20
  21 #include "axis.h"
  22 #include "maths.h"
  23
  24 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
  25 #if defined(VERY_FAST_MATH)
  26
  27 // http://lolengine.net/blog/2011/12/21/better-function-approximations
  28 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
  29 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
  30 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
  31 // sin_approx maximum absolute error = 2.305023e-06
  32 // cos_approx maximum absolute error = 2.857298e-06
  33 #define sinPolyCoef3 -1.666568107e-1f
  34 #define sinPolyCoef5  8.312366210e-3f
  35 #define sinPolyCoef7 -1.849218155e-4f
  36 #define sinPolyCoef9  0
  37 #else
  38 #define sinPolyCoef3 -1.666665710e-1f                                          // Double: -1.666665709650470145824129400050267289858e-1
  39 #define sinPolyCoef5  8.333017292e-3f                                          // Double:  8.333017291562218127986291618761571373087e-3
  40 #define sinPolyCoef7 -1.980661520e-4f                                          // Double: -1.980661520135080504411629636078917643846e-4
  41 #define sinPolyCoef9  2.600054768e-6f                                          // Double:  2.600054767890361277123254766503271638682e-6
  42 #endif
  43 float sin_approx(float x)
  44 {
  45     int32_t xint = x;
  46     if (xint < -32 || xint > 32) return 0.0f;                               // Stop here on error input (5 * 360 Deg)
  47     while (x >  M_PIf) x -= (2.0f * M_PIf);                                 // always wrap input angle to -PI..PI
  48     while (x < -M_PIf) x += (2.0f * M_PIf);
  49     if (x >  (0.5f * M_PIf)) x =  (0.5f * M_PIf) - (x - (0.5f * M_PIf));   // We just pick -90..+90 Degree
  50     else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
  51     float x2 = x * x;
  52     return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
  53 }
  54
  55 float cos_approx(float x)
  56 {
  57     return sin_approx(x + (0.5f * M_PIf));
  58 }
  59
  60 // Initial implementation by Crashpilot1000 (https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292)
  61 // Polynomial coefficients by Andor (http://www.dsprelated.com/showthread/comp.dsp/21872-1.php) optimized by Ledvinap to save one multiplication
  62 // Max absolute error 0,000027 degree
  63 // atan2_approx maximum absolute error = 7.152557e-07 rads (4.098114e-05 degree)
  64 float atan2_approx(float y, float x)
  65 {
  66     #define atanPolyCoef1  3.14551665884836e-07f
  67     #define atanPolyCoef2  0.99997356613987f
  68     #define atanPolyCoef3  0.14744007058297684f
  69     #define atanPolyCoef4  0.3099814292351353f
  70     #define atanPolyCoef5  0.05030176425872175f
  71     #define atanPolyCoef6  0.1471039133652469f
  72     #define atanPolyCoef7  0.6444640676891548f
  73
  74     float res, absX, absY;
  75     absX = fabsf(x);
  76     absY = fabsf(y);
  77     res  = MAX(absX, absY);
  78     if (res) res = MIN(absX, absY) / res;
  79     else res = 0.0f;
  80     res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
  81     if (absY > absX) res = (M_PIf / 2.0f) - res;
  82     if (x < 0) res = M_PIf - res;
  83     if (y < 0) res = -res;
  84     return res;
  85 }
  86
  87 // http://http.developer.nvidia.com/Cg/acos.html
  88 // Handbook of Mathematical Functions
  89 // M. Abramowitz and I.A. Stegun, Ed.
  90 // acos_approx maximum absolute error = 6.760856e-05 rads (3.873685e-03 degree)
  91 float acos_approx(float x)
  92 {
  93     float xa = fabsf(x);
  94     float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
  95     if (x < 0.0f)
  96         return M_PIf - result;
  97     else
  98         return result;
  99 }
 100 #endif
 101
 102 int32_t applyDeadband(int32_t value, int32_t deadband)
 103 {
 104     if (ABS(value) < deadband) {
 105         value = 0;
 106     } else if (value > 0) {
 107         value -= deadband;
 108     } else if (value < 0) {
 109         value += deadband;
 110     }
 111     return value;
 112 }
 113
 114 int constrain(int amt, int low, int high)
 115 {
 116     if (amt < low)
 117         return low;
 118     else if (amt > high)
 119         return high;
 120     else
 121         return amt;
 122 }
 123
 124 float constrainf(float amt, float low, float high)
 125 {
 126     if (amt < low)
 127         return low;
 128     else if (amt > high)
 129         return high;
 130     else
 131         return amt;
 132 }
 133
 134 void devClear(stdev_t *dev)
 135 {
 136     dev->m_n = 0;
 137 }
 138
 139 void devPush(stdev_t *dev, float x)
 140 {
 141     dev->m_n++;
 142     if (dev->m_n == 1) {
 143         dev->m_oldM = dev->m_newM = x;
 144         dev->m_oldS = 0.0f;
 145     } else {
 146         dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
 147         dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
 148         dev->m_oldM = dev->m_newM;
 149         dev->m_oldS = dev->m_newS;
 150     }
 151 }
 152
 153 float devVariance(stdev_t *dev)
 154 {
 155     return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
 156 }
 157
 158 float devStandardDeviation(stdev_t *dev)
 159 {
 160     return sqrtf(devVariance(dev));
 161 }
 162
 163 float degreesToRadians(int16_t degrees)
 164 {
 165     return degrees * RAD;
 166 }
 167
 168 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
 169     long int a = ((long int) destMax - (long int) destMin) * ((long int) x - (long int) srcMin);
 170     long int b = (long int) srcMax - (long int) srcMin;
 171     return ((a / b) - (destMax - destMin)) + destMax;
 172 }
 173
 174 // Normalize a vector
 175 void normalizeV(struct fp_vector *src, struct fp_vector *dest)
 176 {
 177     float length;
 178
 179     length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
 180     if (length != 0) {
 181         dest->X = src->X / length;
 182         dest->Y = src->Y / length;
 183         dest->Z = src->Z / length;
 184     }
 185 }
 186
 187 void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
 188 {
 189     float cosx, sinx, cosy, siny, cosz, sinz;
 190     float coszcosx, sinzcosx, coszsinx, sinzsinx;
 191
 192     cosx = cos_approx(delta->angles.roll);
 193     sinx = sin_approx(delta->angles.roll);
 194     cosy = cos_approx(delta->angles.pitch);
 195     siny = sin_approx(delta->angles.pitch);
 196     cosz = cos_approx(delta->angles.yaw);
 197     sinz = sin_approx(delta->angles.yaw);
 198
 199     coszcosx = cosz * cosx;
 200     sinzcosx = sinz * cosx;
 201     coszsinx = sinx * cosz;
 202     sinzsinx = sinx * sinz;
 203
 204     matrix[0][X] = cosz * cosy;
 205     matrix[0][Y] = -cosy * sinz;
 206     matrix[0][Z] = siny;
 207     matrix[1][X] = sinzcosx + (coszsinx * siny);
 208     matrix[1][Y] = coszcosx - (sinzsinx * siny);
 209     matrix[1][Z] = -sinx * cosy;
 210     matrix[2][X] = (sinzsinx) - (coszcosx * siny);
 211     matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
 212     matrix[2][Z] = cosy * cosx;
 213 }
 214
 215 // Rotate a vector *v by the euler angles defined by the 3-vector *delta.
 216 void rotateV(struct fp_vector *v, fp_angles_t *delta)
 217 {
 218     struct fp_vector v_tmp = *v;
 219
 220     float matrix[3][3];
 221
 222     buildRotationMatrix(delta, matrix);
 223
 224     v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
 225     v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
 226     v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
 227 }
 228
 229 // Quick median filter implementation
 230 // (c) N. Devillard - 1998
 231 // http://ndevilla.free.fr/median/median.pdf
 232 #define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
 233 #define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
 234 #define QMF_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
 235
 236 int32_t quickMedianFilter3(int32_t * v)
 237 {
 238     int32_t p[3];
 239     QMF_COPY(p, v, 3);
 240
 241     QMF_SORT(p[0], p[1]); QMF_SORT(p[1], p[2]); QMF_SORT(p[0], p[1]) ;
 242     return p[1];
 243 }
 244
 245 int32_t quickMedianFilter5(int32_t * v)
 246 {
 247     int32_t p[5];
 248     QMF_COPY(p, v, 5);
 249
 250     QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
 251     QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
 252     QMF_SORT(p[1], p[2]);
 253     return p[2];
 254 }
 255
 256 int32_t quickMedianFilter7(int32_t * v)
 257 {
 258     int32_t p[7];
 259     QMF_COPY(p, v, 7);
 260
 261     QMF_SORT(p[0], p[5]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[6]);
 262     QMF_SORT(p[2], p[4]); QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[5]);
 263     QMF_SORT(p[2], p[6]); QMF_SORT(p[2], p[3]); QMF_SORT(p[3], p[6]);
 264     QMF_SORT(p[4], p[5]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[3]);
 265     QMF_SORT(p[3], p[4]);
 266     return p[3];
 267 }
 268
 269 int32_t quickMedianFilter9(int32_t * v)
 270 {
 271     int32_t p[9];
 272     QMF_COPY(p, v, 9);
 273
 274     QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
 275     QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[6], p[7]);
 276     QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
 277     QMF_SORT(p[0], p[3]); QMF_SORT(p[5], p[8]); QMF_SORT(p[4], p[7]);
 278     QMF_SORT(p[3], p[6]); QMF_SORT(p[1], p[4]); QMF_SORT(p[2], p[5]);
 279     QMF_SORT(p[4], p[7]); QMF_SORT(p[4], p[2]); QMF_SORT(p[6], p[4]);
 280     QMF_SORT(p[4], p[2]);
 281     return p[4];
 282 }
 283
 284 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
 285 {
 286     for (int i = 0; i < count; i++) {
 287         dest[i] = array1[i] - array2[i];
 288     }
 289 }