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 */ 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 */ 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 */ 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 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 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 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 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 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 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 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 */ 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 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 */ 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 */ 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 */ 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 */ 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 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 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 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 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 */ 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 */ 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
