1 /*
2 $License:
3 Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16 $
17 */
18 #include "mlmath.h"
19 #include "mlMathFunc.h"
20 #include "mlinclude.h"
21
22 /**
23 * Performs one iteration of the filter, generating a new y(0)
24 * 1 / N / N \\
25 * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length
26 * a(0) \k=0 \ k=1 //
27 *
28 * The filters A and B should be (sizeof(long) * state->length).
29 * The state variables x and y should be (sizeof(long) * (state->length - 1))
30 *
31 * The state variables x and y should be initialized prior to the first call
32 *
33 * @param state Contains the length of the filter, filter coefficients and
34 * filter state variables x and y.
35 * @param x New input into the filter.
36 */
inv_filter_long(struct filter_long * state,long x)37 void inv_filter_long(struct filter_long *state, long x)
38 {
39 const long *b = state->b;
40 const long *a = state->a;
41 long length = state->length;
42 long long tmp;
43 int ii;
44
45 /* filter */
46 tmp = (long long)x *(b[0]);
47 for (ii = 0; ii < length - 1; ii++) {
48 tmp += ((long long)state->x[ii] * (long long)(b[ii + 1]));
49 }
50 for (ii = 0; ii < length - 1; ii++) {
51 tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1]));
52 }
53 tmp /= (long long)a[0];
54
55 /* Delay */
56 for (ii = length - 2; ii > 0; ii--) {
57 state->y[ii] = state->y[ii - 1];
58 state->x[ii] = state->x[ii - 1];
59 }
60 /* New values */
61 state->y[0] = (long)tmp;
62 state->x[0] = x;
63 }
64
65 /** Performs a multiply and shift by 29. These are good functions to write in assembly on
66 * with devices with small memory where you want to get rid of the long long which some
67 * assemblers don't handle well
68 * @param[in] a
69 * @param[in] b
70 * @return ((long long)a*b)>>29
71 */
inv_q29_mult(long a,long b)72 long inv_q29_mult(long a, long b)
73 {
74 long long temp;
75 long result;
76 temp = (long long)a *b;
77 result = (long)(temp >> 29);
78 return result;
79 }
80
81 /** Performs a multiply and shift by 30. These are good functions to write in assembly on
82 * with devices with small memory where you want to get rid of the long long which some
83 * assemblers don't handle well
84 * @param[in] a
85 * @param[in] b
86 * @return ((long long)a*b)>>30
87 */
inv_q30_mult(long a,long b)88 long inv_q30_mult(long a, long b)
89 {
90 long long temp;
91 long result;
92 temp = (long long)a *b;
93 result = (long)(temp >> 30);
94 return result;
95 }
96
inv_q_mult(const long * q1,const long * q2,long * qProd)97 void inv_q_mult(const long *q1, const long *q2, long *qProd)
98 {
99 INVENSENSE_FUNC_START;
100 qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] -
101 (long long)q1[2] * q2[2] -
102 (long long)q1[3] * q2[3]) >> 30);
103 qProd[1] =
104 (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] +
105 (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30);
106 qProd[2] =
107 (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] +
108 (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30);
109 qProd[3] =
110 (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] -
111 (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30);
112 }
113
inv_q_add(long * q1,long * q2,long * qSum)114 void inv_q_add(long *q1, long *q2, long *qSum)
115 {
116 INVENSENSE_FUNC_START;
117 qSum[0] = q1[0] + q2[0];
118 qSum[1] = q1[1] + q2[1];
119 qSum[2] = q1[2] + q2[2];
120 qSum[3] = q1[3] + q2[3];
121 }
122
inv_q_normalize(long * q)123 void inv_q_normalize(long *q)
124 {
125 INVENSENSE_FUNC_START;
126 double normSF = 0;
127 int i;
128 for (i = 0; i < 4; i++) {
129 normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L;
130 }
131 if (normSF > 0) {
132 normSF = 1 / sqrt(normSF);
133 for (i = 0; i < 4; i++) {
134 q[i] = (int)((double)q[i] * normSF);
135 }
136 } else {
137 q[0] = 1073741824L;
138 q[1] = 0;
139 q[2] = 0;
140 q[3] = 0;
141 }
142 }
143
inv_q_invert(const long * q,long * qInverted)144 void inv_q_invert(const long *q, long *qInverted)
145 {
146 INVENSENSE_FUNC_START;
147 qInverted[0] = q[0];
148 qInverted[1] = -q[1];
149 qInverted[2] = -q[2];
150 qInverted[3] = -q[3];
151 }
152
inv_q_multf(const float * q1,const float * q2,float * qProd)153 void inv_q_multf(const float *q1, const float *q2, float *qProd)
154 {
155 INVENSENSE_FUNC_START;
156 qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
157 qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
158 qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
159 qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
160 }
161
inv_q_addf(float * q1,float * q2,float * qSum)162 void inv_q_addf(float *q1, float *q2, float *qSum)
163 {
164 INVENSENSE_FUNC_START;
165 qSum[0] = q1[0] + q2[0];
166 qSum[1] = q1[1] + q2[1];
167 qSum[2] = q1[2] + q2[2];
168 qSum[3] = q1[3] + q2[3];
169 }
170
inv_q_normalizef(float * q)171 void inv_q_normalizef(float *q)
172 {
173 INVENSENSE_FUNC_START;
174 float normSF = 0;
175 float xHalf = 0;
176 normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
177 if (normSF < 2) {
178 xHalf = 0.5f * normSF;
179 normSF = normSF * (1.5f - xHalf * normSF * normSF);
180 normSF = normSF * (1.5f - xHalf * normSF * normSF);
181 normSF = normSF * (1.5f - xHalf * normSF * normSF);
182 normSF = normSF * (1.5f - xHalf * normSF * normSF);
183 q[0] *= normSF;
184 q[1] *= normSF;
185 q[2] *= normSF;
186 q[3] *= normSF;
187 } else {
188 q[0] = 1.0;
189 q[1] = 0.0;
190 q[2] = 0.0;
191 q[3] = 0.0;
192 }
193 normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
194 }
195
196 /** Performs a length 4 vector normalization with a square root.
197 * @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero.
198 */
inv_q_norm4(float * q)199 void inv_q_norm4(float *q)
200 {
201 float mag;
202 mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
203 if (mag) {
204 q[0] /= mag;
205 q[1] /= mag;
206 q[2] /= mag;
207 q[3] /= mag;
208 } else {
209 q[0] = 1.f;
210 q[1] = 0.f;
211 q[2] = 0.f;
212 q[3] = 0.f;
213 }
214 }
215
inv_q_invertf(const float * q,float * qInverted)216 void inv_q_invertf(const float *q, float *qInverted)
217 {
218 INVENSENSE_FUNC_START;
219 qInverted[0] = q[0];
220 qInverted[1] = -q[1];
221 qInverted[2] = -q[2];
222 qInverted[3] = -q[3];
223 }
224
225 /**
226 * Converts a quaternion to a rotation matrix.
227 * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
228 * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
229 * First 3 elements of the rotation matrix, represent
230 * the first row of the matrix. Rotation matrix multiplied
231 * by a 3 element column vector transform a vector from Body
232 * to World.
233 */
inv_quaternion_to_rotation(const long * quat,long * rot)234 void inv_quaternion_to_rotation(const long *quat, long *rot)
235 {
236 rot[0] =
237 inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
238 quat[0]) - 1073741824L;
239 rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
240 rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
241 rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
242 rot[4] =
243 inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
244 quat[0]) - 1073741824L;
245 rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
246 rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
247 rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
248 rot[8] =
249 inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
250 quat[0]) - 1073741824L;
251 }
252
253 /** Converts a 32-bit long to a big endian byte stream */
inv_int32_to_big8(long x,unsigned char * big8)254 unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
255 {
256 big8[0] = (unsigned char)((x >> 24) & 0xff);
257 big8[1] = (unsigned char)((x >> 16) & 0xff);
258 big8[2] = (unsigned char)((x >> 8) & 0xff);
259 big8[3] = (unsigned char)(x & 0xff);
260 return big8;
261 }
262
263 /** Converts a big endian byte stream into a 32-bit long */
inv_big8_to_int32(const unsigned char * big8)264 long inv_big8_to_int32(const unsigned char *big8)
265 {
266 long x;
267 x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) |
268 ((long)big8[3]);
269 return x;
270 }
271
272 /** Converts a 16-bit short to a big endian byte stream */
inv_int16_to_big8(short x,unsigned char * big8)273 unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
274 {
275 big8[0] = (unsigned char)((x >> 8) & 0xff);
276 big8[1] = (unsigned char)(x & 0xff);
277 return big8;
278 }
279
inv_matrix_det_inc(float * a,float * b,int * n,int x,int y)280 void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
281 {
282 int k, l, i, j;
283 for (i = 0, k = 0; i < *n; i++, k++) {
284 for (j = 0, l = 0; j < *n; j++, l++) {
285 if (i == x)
286 i++;
287 if (j == y)
288 j++;
289 *(b + 10 * k + l) = *(a + 10 * i + j);
290 }
291 }
292 *n = *n - 1;
293 }
294
inv_matrix_det_incd(double * a,double * b,int * n,int x,int y)295 void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
296 {
297 int k, l, i, j;
298 for (i = 0, k = 0; i < *n; i++, k++) {
299 for (j = 0, l = 0; j < *n; j++, l++) {
300 if (i == x)
301 i++;
302 if (j == y)
303 j++;
304 *(b + 10 * k + l) = *(a + 10 * i + j);
305 }
306 }
307 *n = *n - 1;
308 }
309
inv_matrix_det(float * p,int * n)310 float inv_matrix_det(float *p, int *n)
311 {
312 float d[10][10], sum = 0;
313 int i, j, m;
314 m = *n;
315 if (*n == 2)
316 return (*p ** (p + 11) - *(p + 1) ** (p + 10));
317 for (i = 0, j = 0; j < m; j++) {
318 *n = m;
319 inv_matrix_det_inc(p, &d[0][0], n, i, j);
320 sum =
321 sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0],
322 n);
323 }
324
325 return (sum);
326 }
327
inv_matrix_detd(double * p,int * n)328 double inv_matrix_detd(double *p, int *n)
329 {
330 double d[10][10], sum = 0;
331 int i, j, m;
332 m = *n;
333 if (*n == 2)
334 return (*p ** (p + 11) - *(p + 1) ** (p + 10));
335 for (i = 0, j = 0; j < m; j++) {
336 *n = m;
337 inv_matrix_det_incd(p, &d[0][0], n, i, j);
338 sum =
339 sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0],
340 n);
341 }
342
343 return (sum);
344 }
345
346 /** Wraps angle from (-M_PI,M_PI]
347 * @param[in] ang Angle in radians to wrap
348 * @return Wrapped angle from (-M_PI,M_PI]
349 */
inv_wrap_angle(float ang)350 float inv_wrap_angle(float ang)
351 {
352 if (ang > M_PI)
353 return ang - 2 * (float)M_PI;
354 else if (ang <= -(float)M_PI)
355 return ang + 2 * (float)M_PI;
356 else
357 return ang;
358 }
359
360 /** Finds the minimum angle difference ang1-ang2 such that difference
361 * is between [-M_PI,M_PI]
362 * @param[in] ang1
363 * @param[in] ang2
364 * @return angle difference ang1-ang2
365 */
inv_angle_diff(float ang1,float ang2)366 float inv_angle_diff(float ang1, float ang2)
367 {
368 float d;
369 ang1 = inv_wrap_angle(ang1);
370 ang2 = inv_wrap_angle(ang2);
371 d = ang1 - ang2;
372 if (d > M_PI)
373 d -= 2 * (float)M_PI;
374 else if (d < -(float)M_PI)
375 d += 2 * (float)M_PI;
376 return d;
377 }
378