?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Version 1.0 bump
[inav/snaewe.git] / src / main / common / maths.c
blob71dc6f40ef398c68e9537220f8bec48ed34b8976
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 // http://lolengine.net/blog/2011/12/21/better-function-approximations
25 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
26 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
27 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
28 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
29 #if defined(VERY_FAST_MATH)
30 #define sinPolyCoef3 -1.666568107e-1f
31 #define sinPolyCoef5 8.312366210e-3f
32 #define sinPolyCoef7 -1.849218155e-4f
33 #define sinPolyCoef9 0
34 #else
35 #define sinPolyCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1
36 #define sinPolyCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3
37 #define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
38 #define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
39 #endif
40
41 float sin_approx(float x)
42 {
43 int32_t xint = x;
44 if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
45 while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
46 while (x < -M_PIf) x += (2.0f * M_PIf);
47 if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
48 else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
49 float x2 = x * x;
50 return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
51 }
52
53 float cos_approx(float x)
54 {
55 return sin_approx(x + (0.5f * M_PIf));
56 }
57
58 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292
59 // http://http.developer.nvidia.com/Cg/atan2.html (not working correctly!)
60 // Poly coefficients by @ledvinap (https://github.com/cleanflight/cleanflight/pull/1107)
61 // Max absolute error 0,000027 degree
62 float atan2_approx(float y, float x)
63 {
64 #define atanPolyCoef1 3.14551665884836e-07f
65 #define atanPolyCoef2 0.99997356613987f
66 #define atanPolyCoef3 0.14744007058297684f
67 #define atanPolyCoef4 0.3099814292351353f
68 #define atanPolyCoef5 0.05030176425872175f
69 #define atanPolyCoef6 0.1471039133652469f
70 #define atanPolyCoef7 0.6444640676891548f
71
72 float res, absX, absY;
73 absX = fabsf(x);
74 absY = fabsf(y);
75 res = MAX(absX, absY);
76 if (res) res = MIN(absX, absY) / res;
77 else res = 0.0f;
78 res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
79 if (absY > absX) res = (M_PIf / 2.0f) - res;
80 if (x < 0) res = M_PIf - res;
81 if (y < 0) res = -res;
82 return res;
83 }
84
85 // http://http.developer.nvidia.com/Cg/acos.html
86 // Handbook of Mathematical Functions
87 // M. Abramowitz and I.A. Stegun, Ed.
88 // Absolute error <= 6.7e-5
89 float acos_approx(float x)
90 {
91 float xa = fabsf(x);
92 float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
93 if (x < 0.0f)
94 return M_PIf - result;
95 else
96 return result;
97 }
98 #endif
99
100 int32_t wrap_18000(int32_t angle)
101 {
102 if (angle > 18000)
103 angle -= 36000;
104 if (angle < -18000)
105 angle += 36000;
106 return angle;
107 }
108
109 int32_t wrap_36000(int32_t angle)
110 {
111 if (angle > 36000)
112 angle -= 36000;
113 if (angle < 0)
114 angle += 36000;
115 return angle;
116 }
117
118 int32_t applyDeadband(int32_t value, int32_t deadband)
119 {
120 if (ABS(value) < deadband) {
121 value = 0;
122 } else if (value > 0) {
123 value -= deadband;
124 } else if (value < 0) {
125 value += deadband;
126 }
127 return value;
128 }
129
130 int constrain(int amt, int low, int high)
131 {
132 if (amt < low)
133 return low;
134 else if (amt > high)
135 return high;
136 else
137 return amt;
138 }
139
140 float constrainf(float amt, float low, float 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 void devClear(stdev_t *dev)
151 {
152 dev->m_n = 0;
153 }
154
155 void devPush(stdev_t *dev, float x)
156 {
157 dev->m_n++;
158 if (dev->m_n == 1) {
159 dev->m_oldM = dev->m_newM = x;
160 dev->m_oldS = 0.0f;
161 } else {
162 dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
163 dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
164 dev->m_oldM = dev->m_newM;
165 dev->m_oldS = dev->m_newS;
166 }
167 }
168
169 float devVariance(stdev_t *dev)
170 {
171 return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
172 }
173
174 float devStandardDeviation(stdev_t *dev)
175 {
176 return sqrtf(devVariance(dev));
177 }
178
179 float degreesToRadians(int16_t degrees)
180 {
181 return degrees * RAD;
182 }
183
184 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
185 long int a = ((long int) destMax - (long int) destMin) * ((long int) x - (long int) srcMin);
186 long int b = (long int) srcMax - (long int) srcMin;
187 return ((a / b) - (destMax - destMin)) + destMax;
188 }
189
190 // Normalize a vector
191 void normalizeV(struct fp_vector *src, struct fp_vector *dest)
192 {
193 float length;
194
195 length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
196 if (length != 0) {
197 dest->X = src->X / length;
198 dest->Y = src->Y / length;
199 dest->Z = src->Z / length;
200 }
201 }
202
203 void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
204 {
205 float cosx, sinx, cosy, siny, cosz, sinz;
206 float coszcosx, sinzcosx, coszsinx, sinzsinx;
207
208 cosx = cos_approx(delta->angles.roll);
209 sinx = sin_approx(delta->angles.roll);
210 cosy = cos_approx(delta->angles.pitch);
211 siny = sin_approx(delta->angles.pitch);
212 cosz = cos_approx(delta->angles.yaw);
213 sinz = sin_approx(delta->angles.yaw);
214
215 coszcosx = cosz * cosx;
216 sinzcosx = sinz * cosx;
217 coszsinx = sinx * cosz;
218 sinzsinx = sinx * sinz;
219
220 matrix[0][X] = cosz * cosy;
221 matrix[0][Y] = -cosy * sinz;
222 matrix[0][Z] = siny;
223 matrix[1][X] = sinzcosx + (coszsinx * siny);
224 matrix[1][Y] = coszcosx - (sinzsinx * siny);
225 matrix[1][Z] = -sinx * cosy;
226 matrix[2][X] = (sinzsinx) - (coszcosx * siny);
227 matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
228 matrix[2][Z] = cosy * cosx;
229 }
230
231 // Rotate a vector *v by the euler angles defined by the 3-vector *delta.
232 void rotateV(struct fp_vector *v, fp_angles_t *delta)
233 {
234 struct fp_vector v_tmp = *v;
235
236 float matrix[3][3];
237
238 buildRotationMatrix(delta, matrix);
239
240 v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
241 v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
242 v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
243 }
244
245 // Quick median filter implementation
246 // (c) N. Devillard - 1998
247 // http://ndevilla.free.fr/median/median.pdf
248 #define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
249 #define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
250 #define QMF_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
251
252 int32_t quickMedianFilter3(int32_t * v)
253 {
254 int32_t p[3];
255 QMF_COPY(p, v, 3);
256
257 QMF_SORT(p[0], p[1]); QMF_SORT(p[1], p[2]); QMF_SORT(p[0], p[1]) ;
258 return p[1];
259 }
260
261 int32_t quickMedianFilter5(int32_t * v)
262 {
263 int32_t p[5];
264 QMF_COPY(p, v, 5);
265
266 QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
267 QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
268 QMF_SORT(p[1], p[2]);
269 return p[2];
270 }
271
272 int32_t quickMedianFilter7(int32_t * v)
273 {
274 int32_t p[7];
275 QMF_COPY(p, v, 7);
276
277 QMF_SORT(p[0], p[5]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[6]);
278 QMF_SORT(p[2], p[4]); QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[5]);
279 QMF_SORT(p[2], p[6]); QMF_SORT(p[2], p[3]); QMF_SORT(p[3], p[6]);
280 QMF_SORT(p[4], p[5]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[3]);
281 QMF_SORT(p[3], p[4]);
282 return p[3];
283 }
284
285 int32_t quickMedianFilter9(int32_t * v)
286 {
287 int32_t p[9];
288 QMF_COPY(p, v, 9);
289
290 QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
291 QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[6], p[7]);
292 QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
293 QMF_SORT(p[0], p[3]); QMF_SORT(p[5], p[8]); QMF_SORT(p[4], p[7]);
294 QMF_SORT(p[3], p[6]); QMF_SORT(p[1], p[4]); QMF_SORT(p[2], p[5]);
295 QMF_SORT(p[4], p[7]); QMF_SORT(p[4], p[2]); QMF_SORT(p[6], p[4]);
296 QMF_SORT(p[4], p[2]);
297 return p[4];
298 }
299
300 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
301 {
302 for (int i = 0; i < count; i++) {
303 dest[i] = array1[i] - array2[i];
304 }
305 }
306
307 /**
308 * Sensor offset calculation code based on Freescale's AN4246
309 * Initial implementation by @HaukeRa
310 * Modified to be re-usable by @DigitalEntity
311 */
312 void sensorCalibrationResetState(sensorCalibrationState_t * state)
313 {
314 for(int i = 0; i < 4; i++){
315 for(int j = 0; j < 4; j++){
316 state->XtX[i][j] = 0;
317 }
318
319 state->XtY[i] = 0;
320 }
321 }
322
323 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int16_t sample[3])
324 {
325 state->XtX[0][0] += (float)sample[0] * sample[0];
326 state->XtX[0][1] += (float)sample[0] * sample[1];
327 state->XtX[0][2] += (float)sample[0] * sample[2];
328 state->XtX[0][3] += (float)sample[0];
329
330 state->XtX[1][0] += (float)sample[1] * sample[0];
331 state->XtX[1][1] += (float)sample[1] * sample[1];
332 state->XtX[1][2] += (float)sample[1] * sample[2];
333 state->XtX[1][3] += (float)sample[1];
334
335 state->XtX[2][0] += (float)sample[2] * sample[0];
336 state->XtX[2][1] += (float)sample[2] * sample[1];
337 state->XtX[2][2] += (float)sample[2] * sample[2];
338 state->XtX[2][3] += (float)sample[2];
339
340 state->XtX[3][0] += (float)sample[0];
341 state->XtX[3][1] += (float)sample[1];
342 state->XtX[3][2] += (float)sample[2];
343 state->XtX[3][3] += 1;
344
345 float squareSum = ((float)sample[0] * sample[0]) + ((float)sample[1] * sample[1]) + ((float)sample[2] * sample[2]);
346 state->XtY[0] += sample[0] * squareSum;
347 state->XtY[1] += sample[1] * squareSum;
348 state->XtY[2] += sample[2] * squareSum;
349 state->XtY[3] += squareSum;
350 }
351
352 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int16_t sample[3], int target)
353 {
354 for (int i = 0; i < 3; i++) {
355 float scaledSample = (float)sample[i] / (float)target;
356 state->XtX[axis][i] += scaledSample * scaledSample;
357 state->XtX[3][i] += scaledSample * scaledSample;
358 }
359
360 state->XtX[axis][3] += 1;
361 state->XtY[axis] += 1;
362 state->XtY[3] += 1;
363 }
364
365 static void sensorCalibration_gaussLR(float mat[4][4]) {
366 uint8_t n = 4;
367 int i, j, k;
368 for (i = 0; i < 4; i++) {
369 // Determine R
370 for (j = i; j < 4; j++) {
371 for (k = 0; k < i; k++) {
372 mat[i][j] -= mat[i][k] * mat[k][j];
373 }
374 }
375 // Determine L
376 for (j = i + 1; j < n; j++) {
377 for (k = 0; k < i; k++) {
378 mat[j][i] -= mat[j][k] * mat[k][i];
379 }
380 mat[j][i] /= mat[i][i];
381 }
382 }
383 }
384
385 void sensorCalibration_ForwardSubstitution(float LR[4][4], float y[4], float b[4]) {
386 int i, k;
387 for (i = 0; i < 4; ++i) {
388 y[i] = b[i];
389 for (k = 0; k < i; ++k) {
390 y[i] -= LR[i][k] * y[k];
391 }
392 //y[i] /= MAT_ELEM_AT(LR,i,i); //Do not use, LR(i,i) is 1 anyways and not stored in this matrix
393 }
394 }
395
396 void sensorCalibration_BackwardSubstitution(float LR[4][4], float x[4], float y[4]) {
397 int i, k;
398 for (i = 3 ; i >= 0; --i) {
399 x[i] = y[i];
400 for (k = i + 1; k < 4; ++k) {
401 x[i] -= LR[i][k] * x[k];
402 }
403 x[i] /= LR[i][i];
404 }
405 }
406
407 // solve linear equation
408 // https://en.wikipedia.org/wiki/Gaussian_elimination
409 static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
410 int i;
411 float y[4];
412
413 sensorCalibration_gaussLR(A);
414
415 for (i = 0; i < 4; ++i) {
416 y[i] = 0;
417 }
418
419 sensorCalibration_ForwardSubstitution(A, y, b);
420 sensorCalibration_BackwardSubstitution(A, x, y);
421 }
422
423 void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
424 {
425 float beta[4];
426 sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
427
428 for (int i = 0; i < 3; i++) {
429 result[i] = beta[i] / 2;
430 }
431 }
432
433 void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
434 {
435 float beta[4];
436 sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
437
438 for (int i = 0; i < 3; i++) {
439 result[i] = sqrtf(beta[i]);
440 }
441 }