?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
remove test code
[inav.git] / src / main / common / maths.c
blob993634d902dc2783d0ddd44dbf1bce0aa86cbc94
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 <string.h>
20 #include <math.h>
21
22 #include "axis.h"
23 #include "maths.h"
24 #include "vector.h"
25 #include "quaternion.h"
26 #include "platform.h"
27
28 #ifdef USE_ARM_MATH
29 #include "arm_math.h"
30 #endif
31
32 // http://lolengine.net/blog/2011/12/21/better-function-approximations
33 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
34 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
35 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
36 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
37 #if defined(VERY_FAST_MATH)
38 #define sinPolyCoef3 -1.666568107e-1f
39 #define sinPolyCoef5 8.312366210e-3f
40 #define sinPolyCoef7 -1.849218155e-4f
41 #define sinPolyCoef9 0
42 #else
43 #define sinPolyCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1
44 #define sinPolyCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3
45 #define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
46 #define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
47 #endif
48
49 float sin_approx(float x)
50 {
51 int32_t xint = x;
52 if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
53 while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
54 while (x < -M_PIf) x += (2.0f * M_PIf);
55 if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
56 else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
57 float x2 = x * x;
58 return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
59 }
60
61 float cos_approx(float x)
62 {
63 return sin_approx(x + (0.5f * M_PIf));
64 }
65
66 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292
67 // http://http.developer.nvidia.com/Cg/atan2.html (not working correctly!)
68 // Poly coefficients by @ledvinap (https://github.com/cleanflight/cleanflight/pull/1107)
69 // Max absolute error 0,000027 degree
70 float atan2_approx(float y, float x)
71 {
72 #define atanPolyCoef1 3.14551665884836e-07f
73 #define atanPolyCoef2 0.99997356613987f
74 #define atanPolyCoef3 0.14744007058297684f
75 #define atanPolyCoef4 0.3099814292351353f
76 #define atanPolyCoef5 0.05030176425872175f
77 #define atanPolyCoef6 0.1471039133652469f
78 #define atanPolyCoef7 0.6444640676891548f
79
80 float res, absX, absY;
81 absX = fabsf(x);
82 absY = fabsf(y);
83 res = MAX(absX, absY);
84 if (res) res = MIN(absX, absY) / res;
85 else res = 0.0f;
86 res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
87 if (absY > absX) res = (M_PIf / 2.0f) - res;
88 if (x < 0) res = M_PIf - res;
89 if (y < 0) res = -res;
90 return res;
91 }
92
93 // http://http.developer.nvidia.com/Cg/acos.html
94 // Handbook of Mathematical Functions
95 // M. Abramowitz and I.A. Stegun, Ed.
96 // Absolute error <= 6.7e-5
97 float acos_approx(float x)
98 {
99 float xa = fabsf(x);
100 float result = fast_fsqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
101 if (x < 0.0f)
102 return M_PIf - result;
103 else
104 return result;
105 }
106 #endif
107
108 int gcd(int num, int denom)
109 {
110 if (denom == 0) {
111 return num;
112 }
113
114 return gcd(denom, num % denom);
115 }
116
117 int32_t wrap_18000(int32_t angle)
118 {
119 if (angle > 18000)
120 angle -= 36000;
121 if (angle < -18000)
122 angle += 36000;
123 return angle;
124 }
125
126 int16_t wrap_180(int16_t angle)
127 {
128 if (angle > 180)
129 angle -= 360;
130 if (angle < -180)
131 angle += 360;
132 return angle;
133 }
134
135 int32_t wrap_36000(int32_t angle)
136 {
137 if (angle >= 36000)
138 angle -= 36000;
139 if (angle < 0)
140 angle += 36000;
141 return angle;
142 }
143
144 int32_t applyDeadband(int32_t value, int32_t deadband)
145 {
146 if (ABS(value) < deadband) {
147 value = 0;
148 } else if (value > 0) {
149 value -= deadband;
150 } else if (value < 0) {
151 value += deadband;
152 }
153 return value;
154 }
155
156 int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max)
157 {
158 if (ABS(value) < deadband) {
159 value = 0;
160 } else if (value > 0) {
161 value = scaleRange(value - deadband, 0, max - deadband, 0, max);
162 } else if (value < 0) {
163 value = scaleRange(value + deadband, min + deadband, 0, min, 0);
164 }
165 return value;
166 }
167
168 int32_t constrain(int32_t amt, int32_t low, int32_t high)
169 {
170 if (amt < low)
171 return low;
172 else if (amt > high)
173 return high;
174 else
175 return amt;
176 }
177
178 float constrainf(float amt, float low, float high)
179 {
180 if (amt < low)
181 return low;
182 else if (amt > high)
183 return high;
184 else
185 return amt;
186 }
187
188 void devClear(stdev_t *dev)
189 {
190 dev->m_n = 0;
191 }
192
193 void devPush(stdev_t *dev, float x)
194 {
195 dev->m_n++;
196 if (dev->m_n == 1) {
197 dev->m_oldM = dev->m_newM = x;
198 dev->m_oldS = 0.0f;
199 } else {
200 dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
201 dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
202 dev->m_oldM = dev->m_newM;
203 dev->m_oldS = dev->m_newS;
204 }
205 }
206
207 float devVariance(stdev_t *dev)
208 {
209 return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
210 }
211
212 float devStandardDeviation(stdev_t *dev)
213 {
214 return fast_fsqrtf(devVariance(dev));
215 }
216
217 float degreesToRadians(int16_t degrees)
218 {
219 return degrees * RAD;
220 }
221
222 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
223 long int a = ((long int) destMax - (long int) destMin) * ((long int) x - (long int) srcMin);
224 long int b = (long int) srcMax - (long int) srcMin;
225 return ((a / b) + destMin);
226 }
227
228 float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
229 float a = (destMax - destMin) * (x - srcMin);
230 float b = srcMax - srcMin;
231 return ((a / b) + destMin);
232 }
233
234 // Build rMat from Tait–Bryan angles (convention X1, Y2, Z3)
235 void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles)
236 {
237 float cosx, sinx, cosy, siny, cosz, sinz;
238 float coszcosx, sinzcosx, coszsinx, sinzsinx;
239
240 cosx = cos_approx(angles->angles.roll);
241 sinx = sin_approx(angles->angles.roll);
242 cosy = cos_approx(angles->angles.pitch);
243 siny = sin_approx(angles->angles.pitch);
244 cosz = cos_approx(angles->angles.yaw);
245 sinz = sin_approx(angles->angles.yaw);
246
247 coszcosx = cosz * cosx;
248 sinzcosx = sinz * cosx;
249 coszsinx = sinx * cosz;
250 sinzsinx = sinx * sinz;
251
252 rmat->m[0][X] = cosz * cosy;
253 rmat->m[0][Y] = -cosy * sinz;
254 rmat->m[0][Z] = siny;
255 rmat->m[1][X] = sinzcosx + (coszsinx * siny);
256 rmat->m[1][Y] = coszcosx - (sinzsinx * siny);
257 rmat->m[1][Z] = -sinx * cosy;
258 rmat->m[2][X] = (sinzsinx) - (coszcosx * siny);
259 rmat->m[2][Y] = (coszsinx) + (sinzcosx * siny);
260 rmat->m[2][Z] = cosy * cosx;
261 }
262
263 void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a)
264 {
265 const float sang = sin_approx(a->angle);
266 const float cang = cos_approx(a->angle);
267 const float C = 1.0f - cang;
268
269 const float xC = a->axis.x * C;
270 const float yC = a->axis.y * C;
271 const float zC = a->axis.z * C;
272 const float xxC = a->axis.x * xC;
273 const float yyC = a->axis.y * yC;
274 const float zzC = a->axis.z * zC;
275 const float xyC = a->axis.x * yC;
276 const float yzC = a->axis.y * zC;
277 const float zxC = a->axis.z * xC;
278 const float xs = a->axis.x * sang;
279 const float ys = a->axis.y * sang;
280 const float zs = a->axis.z * sang;
281
282 rmat->m[0][X] = xxC + cang;
283 rmat->m[0][Y] = xyC - zs;
284 rmat->m[0][Z] = zxC + ys;
285
286 rmat->m[1][X] = zxC + ys;
287 rmat->m[1][Y] = yyC + cang;
288 rmat->m[1][Z] = yzC - xs;
289
290 rmat->m[2][X] = zxC - ys;
291 rmat->m[2][Y] = yzC + xs;
292 rmat->m[2][Z] = zzC + cang;
293 }
294
295 // Quick median filter implementation
296 // (c) N. Devillard - 1998
297 // http://ndevilla.free.fr/median/median.pdf
298 #define QMF_SORT(type,a,b) { if ((a)>(b)) QMF_SWAP(type, (a),(b)); }
299 #define QMF_SWAP(type,a,b) { type temp=(a);(a)=(b);(b)=temp; }
300
301 int32_t quickMedianFilter3(int32_t * v)
302 {
303 int32_t p[3];
304 memcpy(p, v, sizeof(p));
305
306 QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[0], p[1]) ;
307 return p[1];
308 }
309
310 int16_t quickMedianFilter3_16(int16_t * v)
311 {
312 int16_t p[3];
313 memcpy(p, v, sizeof(p));
314
315 QMF_SORT(int16_t, p[0], p[1]); QMF_SORT(int16_t, p[1], p[2]); QMF_SORT(int16_t, p[0], p[1]) ;
316 return p[1];
317 }
318
319 int32_t quickMedianFilter5(int32_t * v)
320 {
321 int32_t p[5];
322 memcpy(p, v, sizeof(p));
323
324 QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[4]); QMF_SORT(int32_t, p[0], p[3]);
325 QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[2], p[3]);
326 QMF_SORT(int32_t, p[1], p[2]);
327 return p[2];
328 }
329
330 int16_t quickMedianFilter5_16(int16_t * v)
331 {
332 int16_t p[5];
333 memcpy(p, v, sizeof(p));
334
335 QMF_SORT(int16_t, p[0], p[1]); QMF_SORT(int16_t, p[3], p[4]); QMF_SORT(int16_t, p[0], p[3]);
336 QMF_SORT(int16_t, p[1], p[4]); QMF_SORT(int16_t, p[1], p[2]); QMF_SORT(int16_t, p[2], p[3]);
337 QMF_SORT(int16_t, p[1], p[2]);
338 return p[2];
339 }
340
341 int32_t quickMedianFilter7(int32_t * v)
342 {
343 int32_t p[7];
344 memcpy(p, v, sizeof(p));
345
346 QMF_SORT(int32_t, p[0], p[5]); QMF_SORT(int32_t, p[0], p[3]); QMF_SORT(int32_t, p[1], p[6]);
347 QMF_SORT(int32_t, p[2], p[4]); QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[5]);
348 QMF_SORT(int32_t, p[2], p[6]); QMF_SORT(int32_t, p[2], p[3]); QMF_SORT(int32_t, p[3], p[6]);
349 QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[1], p[3]);
350 QMF_SORT(int32_t, p[3], p[4]);
351 return p[3];
352 }
353
354 int32_t quickMedianFilter9(int32_t * v)
355 {
356 int32_t p[9];
357 memcpy(p, v, sizeof(p));
358
359 QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[7], p[8]);
360 QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[4]); QMF_SORT(int32_t, p[6], p[7]);
361 QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[7], p[8]);
362 QMF_SORT(int32_t, p[0], p[3]); QMF_SORT(int32_t, p[5], p[8]); QMF_SORT(int32_t, p[4], p[7]);
363 QMF_SORT(int32_t, p[3], p[6]); QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[2], p[5]);
364 QMF_SORT(int32_t, p[4], p[7]); QMF_SORT(int32_t, p[4], p[2]); QMF_SORT(int32_t, p[6], p[4]);
365 QMF_SORT(int32_t, p[4], p[2]);
366 return p[4];
367 }
368
369 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
370 {
371 for (int i = 0; i < count; i++) {
372 dest[i] = array1[i] - array2[i];
373 }
374 }
375
376 /**
377 * Sensor offset calculation code based on Freescale's AN4246
378 * Initial implementation by @HaukeRa
379 * Modified to be re-usable by @DigitalEntity
380 */
381 void sensorCalibrationResetState(sensorCalibrationState_t * state)
382 {
383 for (int i = 0; i < 4; i++){
384 for (int j = 0; j < 4; j++){
385 state->XtX[i][j] = 0;
386 }
387
388 state->XtY[i] = 0;
389 }
390 }
391
392 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3])
393 {
394 state->XtX[0][0] += sample[0] * sample[0];
395 state->XtX[0][1] += sample[0] * sample[1];
396 state->XtX[0][2] += sample[0] * sample[2];
397 state->XtX[0][3] += sample[0];
398
399 state->XtX[1][0] += sample[1] * sample[0];
400 state->XtX[1][1] += sample[1] * sample[1];
401 state->XtX[1][2] += sample[1] * sample[2];
402 state->XtX[1][3] += sample[1];
403
404 state->XtX[2][0] += sample[2] * sample[0];
405 state->XtX[2][1] += sample[2] * sample[1];
406 state->XtX[2][2] += sample[2] * sample[2];
407 state->XtX[2][3] += sample[2];
408
409 state->XtX[3][0] += sample[0];
410 state->XtX[3][1] += sample[1];
411 state->XtX[3][2] += sample[2];
412 state->XtX[3][3] += 1;
413
414 float squareSum = (sample[0] * sample[0]) + (sample[1] * sample[1]) + (sample[2] * sample[2]);
415 state->XtY[0] += sample[0] * squareSum;
416 state->XtY[1] += sample[1] * squareSum;
417 state->XtY[2] += sample[2] * squareSum;
418 state->XtY[3] += squareSum;
419 }
420
421 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target)
422 {
423 for (int i = 0; i < 3; i++) {
424 float scaledSample = (float)sample[i] / (float)target;
425 state->XtX[axis][i] += scaledSample * scaledSample;
426 state->XtX[3][i] += scaledSample * scaledSample;
427 }
428
429 state->XtX[axis][3] += 1;
430 state->XtY[axis] += 1;
431 state->XtY[3] += 1;
432 }
433
434 static void sensorCalibration_gaussLR(float mat[4][4]) {
435 uint8_t n = 4;
436 int i, j, k;
437 for (i = 0; i < 4; i++) {
438 // Determine R
439 for (j = i; j < 4; j++) {
440 for (k = 0; k < i; k++) {
441 mat[i][j] -= mat[i][k] * mat[k][j];
442 }
443 }
444 // Determine L
445 for (j = i + 1; j < n; j++) {
446 for (k = 0; k < i; k++) {
447 mat[j][i] -= mat[j][k] * mat[k][i];
448 }
449 mat[j][i] /= mat[i][i];
450 }
451 }
452 }
453
454 void sensorCalibration_ForwardSubstitution(float LR[4][4], float y[4], float b[4]) {
455 int i, k;
456 for (i = 0; i < 4; ++i) {
457 y[i] = b[i];
458 for (k = 0; k < i; ++k) {
459 y[i] -= LR[i][k] * y[k];
460 }
461 //y[i] /= MAT_ELEM_AT(LR,i,i); //Do not use, LR(i,i) is 1 anyways and not stored in this matrix
462 }
463 }
464
465 void sensorCalibration_BackwardSubstitution(float LR[4][4], float x[4], float y[4]) {
466 int i, k;
467 for (i = 3 ; i >= 0; --i) {
468 x[i] = y[i];
469 for (k = i + 1; k < 4; ++k) {
470 x[i] -= LR[i][k] * x[k];
471 }
472 x[i] /= LR[i][i];
473 }
474 }
475
476 // solve linear equation
477 // https://en.wikipedia.org/wiki/Gaussian_elimination
478 static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
479 int i;
480 float y[4];
481
482 sensorCalibration_gaussLR(A);
483
484 for (i = 0; i < 4; ++i) {
485 y[i] = 0;
486 }
487
488 sensorCalibration_ForwardSubstitution(A, y, b);
489 sensorCalibration_BackwardSubstitution(A, x, y);
490 }
491
492 bool sensorCalibrationValidateResult(const float result[3])
493 {
494 // Validate that result is not INF and not NAN
495 for (int i = 0; i < 3; i++) {
496 if (isnan(result[i]) && isinf(result[i])) {
497 return false;
498 }
499 }
500
501 return true;
502 }
503
504 bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
505 {
506 float beta[4];
507 sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
508
509 for (int i = 0; i < 3; i++) {
510 result[i] = beta[i] / 2;
511 }
512
513 return sensorCalibrationValidateResult(result);
514 }
515
516 bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
517 {
518 float beta[4];
519 sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
520
521 for (int i = 0; i < 3; i++) {
522 result[i] = fast_fsqrtf(beta[i]);
523 }
524
525 return sensorCalibrationValidateResult(result);
526 }
527
528 float gaussian(const float x, const float mu, const float sigma) {
529 return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2)));
530 }
531
532 float bellCurve(const float x, const float curveWidth)
533 {
534 return gaussian(x, 0.0f, curveWidth);
535 }
536
537 /**
538 * @brief Calculate the attenuation of a value using a Gaussian function.
539 * Retuns 1 for input 0 and ~0 for input width.
540 * @param input The input value.
541 * @param width The width of the Gaussian function.
542 * @return The attenuation of the input value.
543 */
544 float attenuation(const float input, const float width) {
545 const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2))
546 return gaussian(input, 0.0f, sigma);
547 }
548
549 float fast_fsqrtf(const float value) {
550 float ret = 0.0f;
551 #ifdef USE_ARM_MATH
552 arm_sqrt_f32(value, &ret);
553 #else
554 ret = sqrtf(value);
555 #endif
556 if (isnan(ret))
557 {
558 return 0.0f;
559 }
560 return ret;
561 }
562
563 // function to calculate the normalization (pythagoras) of a 2-dimensional vector
564 float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement)
565 {
566 return fast_fsqrtf(sq(firstElement) + sq(secondElement));
567 }
568
569 // function to calculate the normalization (pythagoras) of a 3-dimensional vector
570 float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement)
571 {
572 return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
573 }
574
575 #ifdef SITL_BUILD
576
577 /**
578 * @brief Floating-point vector subtraction, equivalent of CMSIS arm_sub_f32.
579 */
580 void arm_sub_f32(
581 float * pSrcA,
582 float * pSrcB,
583 float * pDst,
584 uint32_t blockSize)
585 {
586 for (uint32_t i = 0; i < blockSize; i++) {
587 pDst[i] = pSrcA[i] - pSrcB[i];
588 }
589 }
590
591 /**
592 * @brief Floating-point vector scaling, equivalent of CMSIS arm_scale_f32.
593 */
594 void arm_scale_f32(
595 float * pSrc,
596 float scale,
597 float * pDst,
598 uint32_t blockSize)
599 {
600 for (uint32_t i = 0; i < blockSize; i++) {
601 pDst[i] = pSrc[i] * scale;
602 }
603 }
604
605 /**
606 * @brief Floating-point vector multiplication, equivalent of CMSIS arm_mult_f32.
607 */
608 void arm_mult_f32(
609 float * pSrcA,
610 float * pSrcB,
611 float * pDst,
612 uint32_t blockSize)
613 {
614 for (uint32_t i = 0; i < blockSize; i++) {
615 pDst[i] = pSrcA[i] * pSrcB[i];
616 }
617 }
618
619 #endif
INAV: Navigation-enabled flight control software