?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge pull request #11494 from haslinghuis/dshot_gpio
[betaflight.git] / src / main / common / maths.c
blob104dfd97188c5d06430e558b16f8acd13fabb8e8
1 /*
2 * This file is part of Cleanflight and Betaflight.
3 *
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
9 *
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
17 *
18 * If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 #include <stdint.h>
22 #include <math.h>
23
24 #include "platform.h"
25
26 #include "build/build_config.h"
27
28 #include "axis.h"
29 #include "maths.h"
30
31 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
32 #if defined(VERY_FAST_MATH)
33
34 // http://lolengine.net/blog/2011/12/21/better-function-approximations
35 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
36 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
37 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
38 // sin_approx maximum absolute error = 2.305023e-06
39 // cos_approx maximum absolute error = 2.857298e-06
40 #define sinPolyCoef3 -1.666568107e-1f
41 #define sinPolyCoef5 8.312366210e-3f
42 #define sinPolyCoef7 -1.849218155e-4f
43 #define sinPolyCoef9 0
44 #else
45 #define sinPolyCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1
46 #define sinPolyCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3
47 #define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
48 #define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
49 #endif
50 float sin_approx(float x)
51 {
52 int32_t xint = x;
53 if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
54 while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
55 while (x < -M_PIf) x += (2.0f * M_PIf);
56 if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
57 else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
58 float x2 = x * x;
59 return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
60 }
61
62 float cos_approx(float x)
63 {
64 return sin_approx(x + (0.5f * M_PIf));
65 }
66
67 // Initial implementation by Crashpilot1000 (https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292)
68 // Polynomial coefficients by Andor (http://www.dsprelated.com/showthread/comp.dsp/21872-1.php) optimized by Ledvinap to save one multiplication
69 // Max absolute error 0,000027 degree
70 // atan2_approx maximum absolute error = 7.152557e-07 rads (4.098114e-05 degree)
71 float atan2_approx(float y, float x)
72 {
73 #define atanPolyCoef1 3.14551665884836e-07f
74 #define atanPolyCoef2 0.99997356613987f
75 #define atanPolyCoef3 0.14744007058297684f
76 #define atanPolyCoef4 0.3099814292351353f
77 #define atanPolyCoef5 0.05030176425872175f
78 #define atanPolyCoef6 0.1471039133652469f
79 #define atanPolyCoef7 0.6444640676891548f
80
81 float res, absX, absY;
82 absX = fabsf(x);
83 absY = fabsf(y);
84 res = MAX(absX, absY);
85 if (res) res = MIN(absX, absY) / res;
86 else res = 0.0f;
87 res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
88 if (absY > absX) res = (M_PIf / 2.0f) - res;
89 if (x < 0) res = M_PIf - res;
90 if (y < 0) res = -res;
91 return res;
92 }
93
94 // http://http.developer.nvidia.com/Cg/acos.html
95 // Handbook of Mathematical Functions
96 // M. Abramowitz and I.A. Stegun, Ed.
97 // acos_approx maximum absolute error = 6.760856e-05 rads (3.873685e-03 degree)
98 float acos_approx(float x)
99 {
100 float xa = fabsf(x);
101 float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
102 if (x < 0.0f)
103 return M_PIf - result;
104 else
105 return result;
106 }
107 #endif
108
109 int gcd(int num, int denom)
110 {
111 if (denom == 0) {
112 return num;
113 }
114
115 return gcd(denom, num % denom);
116 }
117
118 int32_t applyDeadband(const int32_t value, const int32_t deadband)
119 {
120 if (ABS(value) < deadband) {
121 return 0;
122 }
123
124 return value >= 0 ? value - deadband : value + deadband;
125 }
126
127 float fapplyDeadband(const float value, const float deadband)
128 {
129 if (fabsf(value) < deadband) {
130 return 0;
131 }
132
133 return value >= 0 ? value - deadband : value + deadband;
134 }
135
136 void devClear(stdev_t *dev)
137 {
138 dev->m_n = 0;
139 }
140
141 void devPush(stdev_t *dev, float x)
142 {
143 dev->m_n++;
144 if (dev->m_n == 1) {
145 dev->m_oldM = dev->m_newM = x;
146 dev->m_oldS = 0.0f;
147 } else {
148 dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
149 dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
150 dev->m_oldM = dev->m_newM;
151 dev->m_oldS = dev->m_newS;
152 }
153 }
154
155 float devVariance(stdev_t *dev)
156 {
157 return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
158 }
159
160 float devStandardDeviation(stdev_t *dev)
161 {
162 return sqrtf(devVariance(dev));
163 }
164
165 float degreesToRadians(int16_t degrees)
166 {
167 return degrees * RAD;
168 }
169
170 int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo) {
171 long int a = ((long int) destTo - (long int) destFrom) * ((long int) x - (long int) srcFrom);
172 long int b = (long int) srcTo - (long int) srcFrom;
173 return (a / b) + destFrom;
174 }
175
176 float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo) {
177 float a = (destTo - destFrom) * (x - srcFrom);
178 float b = srcTo - srcFrom;
179 return (a / b) + destFrom;
180 }
181
182 void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
183 {
184 float cosx, sinx, cosy, siny, cosz, sinz;
185 float coszcosx, sinzcosx, coszsinx, sinzsinx;
186
187 cosx = cos_approx(delta->angles.roll);
188 sinx = sin_approx(delta->angles.roll);
189 cosy = cos_approx(delta->angles.pitch);
190 siny = sin_approx(delta->angles.pitch);
191 cosz = cos_approx(delta->angles.yaw);
192 sinz = sin_approx(delta->angles.yaw);
193
194 coszcosx = cosz * cosx;
195 sinzcosx = sinz * cosx;
196 coszsinx = sinx * cosz;
197 sinzsinx = sinx * sinz;
198
199 rotation->m[0][X] = cosz * cosy;
200 rotation->m[0][Y] = -cosy * sinz;
201 rotation->m[0][Z] = siny;
202 rotation->m[1][X] = sinzcosx + (coszsinx * siny);
203 rotation->m[1][Y] = coszcosx - (sinzsinx * siny);
204 rotation->m[1][Z] = -sinx * cosy;
205 rotation->m[2][X] = (sinzsinx) - (coszcosx * siny);
206 rotation->m[2][Y] = (coszsinx) + (sinzcosx * siny);
207 rotation->m[2][Z] = cosy * cosx;
208 }
209
210 void applyMatrixRotation(float *v, fp_rotationMatrix_t *rotationMatrix)
211 {
212 struct fp_vector *vDest = (struct fp_vector *)v;
213 struct fp_vector vTmp = *vDest;
214
215 vDest->X = (rotationMatrix->m[0][X] * vTmp.X + rotationMatrix->m[1][X] * vTmp.Y + rotationMatrix->m[2][X] * vTmp.Z);
216 vDest->Y = (rotationMatrix->m[0][Y] * vTmp.X + rotationMatrix->m[1][Y] * vTmp.Y + rotationMatrix->m[2][Y] * vTmp.Z);
217 vDest->Z = (rotationMatrix->m[0][Z] * vTmp.X + rotationMatrix->m[1][Z] * vTmp.Y + rotationMatrix->m[2][Z] * vTmp.Z);
218 }
219
220 // Quick median filter implementation
221 // (c) N. Devillard - 1998
222 // http://ndevilla.free.fr/median/median.pdf
223 #define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
224 #define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
225 #define QMF_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
226 #define QMF_SORTF(a,b) { if ((a)>(b)) QMF_SWAPF((a),(b)); }
227 #define QMF_SWAPF(a,b) { float temp=(a);(a)=(b);(b)=temp; }
228
229 int32_t quickMedianFilter3(int32_t * v)
230 {
231 int32_t p[3];
232 QMF_COPY(p, v, 3);
233
234 QMF_SORT(p[0], p[1]); QMF_SORT(p[1], p[2]); QMF_SORT(p[0], p[1]) ;
235 return p[1];
236 }
237
238 int32_t quickMedianFilter5(int32_t * v)
239 {
240 int32_t p[5];
241 QMF_COPY(p, v, 5);
242
243 QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
244 QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
245 QMF_SORT(p[1], p[2]);
246 return p[2];
247 }
248
249 int32_t quickMedianFilter7(int32_t * v)
250 {
251 int32_t p[7];
252 QMF_COPY(p, v, 7);
253
254 QMF_SORT(p[0], p[5]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[6]);
255 QMF_SORT(p[2], p[4]); QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[5]);
256 QMF_SORT(p[2], p[6]); QMF_SORT(p[2], p[3]); QMF_SORT(p[3], p[6]);
257 QMF_SORT(p[4], p[5]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[3]);
258 QMF_SORT(p[3], p[4]);
259 return p[3];
260 }
261
262 int32_t quickMedianFilter9(int32_t * v)
263 {
264 int32_t p[9];
265 QMF_COPY(p, v, 9);
266
267 QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
268 QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[6], p[7]);
269 QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
270 QMF_SORT(p[0], p[3]); QMF_SORT(p[5], p[8]); QMF_SORT(p[4], p[7]);
271 QMF_SORT(p[3], p[6]); QMF_SORT(p[1], p[4]); QMF_SORT(p[2], p[5]);
272 QMF_SORT(p[4], p[7]); QMF_SORT(p[4], p[2]); QMF_SORT(p[6], p[4]);
273 QMF_SORT(p[4], p[2]);
274 return p[4];
275 }
276
277 float quickMedianFilter3f(float * v)
278 {
279 float p[3];
280 QMF_COPY(p, v, 3);
281
282 QMF_SORTF(p[0], p[1]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[0], p[1]) ;
283 return p[1];
284 }
285
286 float quickMedianFilter5f(float * v)
287 {
288 float p[5];
289 QMF_COPY(p, v, 5);
290
291 QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[0], p[3]);
292 QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[2], p[3]);
293 QMF_SORTF(p[1], p[2]);
294 return p[2];
295 }
296
297 float quickMedianFilter7f(float * v)
298 {
299 float p[7];
300 QMF_COPY(p, v, 7);
301
302 QMF_SORTF(p[0], p[5]); QMF_SORTF(p[0], p[3]); QMF_SORTF(p[1], p[6]);
303 QMF_SORTF(p[2], p[4]); QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[5]);
304 QMF_SORTF(p[2], p[6]); QMF_SORTF(p[2], p[3]); QMF_SORTF(p[3], p[6]);
305 QMF_SORTF(p[4], p[5]); QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[3]);
306 QMF_SORTF(p[3], p[4]);
307 return p[3];
308 }
309
310 float quickMedianFilter9f(float * v)
311 {
312 float p[9];
313 QMF_COPY(p, v, 9);
314
315 QMF_SORTF(p[1], p[2]); QMF_SORTF(p[4], p[5]); QMF_SORTF(p[7], p[8]);
316 QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[6], p[7]);
317 QMF_SORTF(p[1], p[2]); QMF_SORTF(p[4], p[5]); QMF_SORTF(p[7], p[8]);
318 QMF_SORTF(p[0], p[3]); QMF_SORTF(p[5], p[8]); QMF_SORTF(p[4], p[7]);
319 QMF_SORTF(p[3], p[6]); QMF_SORTF(p[1], p[4]); QMF_SORTF(p[2], p[5]);
320 QMF_SORTF(p[4], p[7]); QMF_SORTF(p[4], p[2]); QMF_SORTF(p[6], p[4]);
321 QMF_SORTF(p[4], p[2]);
322 return p[4];
323 }
324
325 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
326 {
327 for (int i = 0; i < count; i++) {
328 dest[i] = array1[i] - array2[i];
329 }
330 }
331
332 int16_t qPercent(fix12_t q) {
333 return (100 * q) >> 12;
334 }
335
336 int16_t qMultiply(fix12_t q, int16_t input) {
337 return (input * q) >> 12;
338 }
339
340 fix12_t qConstruct(int16_t num, int16_t den) {
341 return (num << 12) / den;
342 }