diff options
Diffstat (limited to 'libsensors/mlsdk/mllite/mlMathFunc.c')
| -rw-r--r-- | libsensors/mlsdk/mllite/mlMathFunc.c | 377 |
1 files changed, 377 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/mlMathFunc.c b/libsensors/mlsdk/mllite/mlMathFunc.c new file mode 100644 index 0000000..31b42bc --- /dev/null +++ b/libsensors/mlsdk/mllite/mlMathFunc.c @@ -0,0 +1,377 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#include "mlmath.h" +#include "mlMathFunc.h" +#include "mlinclude.h" + +/** + * Performs one iteration of the filter, generating a new y(0) + * 1 / N / N \\ + * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length + * a(0) \k=0 \ k=1 // + * + * The filters A and B should be (sizeof(long) * state->length). + * The state variables x and y should be (sizeof(long) * (state->length - 1)) + * + * The state variables x and y should be initialized prior to the first call + * + * @param state Contains the length of the filter, filter coefficients and + * filter state variables x and y. + * @param x New input into the filter. + */ +void inv_filter_long(struct filter_long *state, long x) +{ + const long *b = state->b; + const long *a = state->a; + long length = state->length; + long long tmp; + int ii; + + /* filter */ + tmp = (long long)x *(b[0]); + for (ii = 0; ii < length - 1; ii++) { + tmp += ((long long)state->x[ii] * (long long)(b[ii + 1])); + } + for (ii = 0; ii < length - 1; ii++) { + tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1])); + } + tmp /= (long long)a[0]; + + /* Delay */ + for (ii = length - 2; ii > 0; ii--) { + state->y[ii] = state->y[ii - 1]; + state->x[ii] = state->x[ii - 1]; + } + /* New values */ + state->y[0] = (long)tmp; + state->x[0] = x; +} + +/** Performs a multiply and shift by 29. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>29 +*/ +long inv_q29_mult(long a, long b) +{ + long long temp; + long result; + temp = (long long)a *b; + result = (long)(temp >> 29); + return result; +} + +/** Performs a multiply and shift by 30. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>30 +*/ +long inv_q30_mult(long a, long b) +{ + long long temp; + long result; + temp = (long long)a *b; + result = (long)(temp >> 30); + return result; +} + +void inv_q_mult(const long *q1, const long *q2, long *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] - + (long long)q1[2] * q2[2] - + (long long)q1[3] * q2[3]) >> 30); + qProd[1] = + (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] + + (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30); + qProd[2] = + (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] + + (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30); + qProd[3] = + (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] - + (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30); +} + +void inv_q_add(long *q1, long *q2, long *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_q_normalize(long *q) +{ + INVENSENSE_FUNC_START; + double normSF = 0; + int i; + for (i = 0; i < 4; i++) { + normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L; + } + if (normSF > 0) { + normSF = 1 / sqrt(normSF); + for (i = 0; i < 4; i++) { + q[i] = (int)((double)q[i] * normSF); + } + } else { + q[0] = 1073741824L; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } +} + +void inv_q_invert(const long *q, long *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +void inv_q_multf(const float *q1, const float *q2, float *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); + qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); + qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); + qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); +} + +void inv_q_addf(float *q1, float *q2, float *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_q_normalizef(float *q) +{ + INVENSENSE_FUNC_START; + float normSF = 0; + float xHalf = 0; + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (normSF < 2) { + xHalf = 0.5f * normSF; + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + q[0] *= normSF; + q[1] *= normSF; + q[2] *= normSF; + q[3] *= normSF; + } else { + q[0] = 1.0; + q[1] = 0.0; + q[2] = 0.0; + q[3] = 0.0; + } + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); +} + +/** Performs a length 4 vector normalization with a square root. +* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero. +*/ +void inv_q_norm4(float *q) +{ + float mag; + mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (mag) { + q[0] /= mag; + q[1] /= mag; + q[2] /= mag; + q[3] /= mag; + } else { + q[0] = 1.f; + q[1] = 0.f; + q[2] = 0.f; + q[3] = 0.f; + } +} + +void inv_q_invertf(const float *q, float *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +/** + * Converts a quaternion to a rotation matrix. + * @param[in] quat 4-element quaternion in fixed point. One is 2^30. + * @param[out] rot Rotation matrix in fixed point. One is 2^30. The + * First 3 elements of the rotation matrix, represent + * the first row of the matrix. Rotation matrix multiplied + * by a 3 element column vector transform a vector from Body + * to World. + */ +void inv_quaternion_to_rotation(const long *quat, long *rot) +{ + rot[0] = + inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], + quat[0]) - 1073741824L; + rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); + rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); + rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); + rot[4] = + inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], + quat[0]) - 1073741824L; + rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); + rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); + rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); + rot[8] = + inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], + quat[0]) - 1073741824L; +} + +/** Converts a 32-bit long to a big endian byte stream */ +unsigned char *inv_int32_to_big8(long x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 24) & 0xff); + big8[1] = (unsigned char)((x >> 16) & 0xff); + big8[2] = (unsigned char)((x >> 8) & 0xff); + big8[3] = (unsigned char)(x & 0xff); + return big8; +} + +/** Converts a big endian byte stream into a 32-bit long */ +long inv_big8_to_int32(const unsigned char *big8) +{ + long x; + x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) | + ((long)big8[3]); + return x; +} + +/** Converts a 16-bit short to a big endian byte stream */ +unsigned char *inv_int16_to_big8(short x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 8) & 0xff); + big8[1] = (unsigned char)(x & 0xff); + return big8; +} + +void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 10 * k + l) = *(a + 10 * i + j); + } + } + *n = *n - 1; +} + +void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 10 * k + l) = *(a + 10 * i + j); + } + } + *n = *n - 1; +} + +float inv_matrix_det(float *p, int *n) +{ + float d[10][10], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 11) - *(p + 1) ** (p + 10)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_inc(p, &d[0][0], n, i, j); + sum = + sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0], + n); + } + + return (sum); +} + +double inv_matrix_detd(double *p, int *n) +{ + double d[10][10], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 11) - *(p + 1) ** (p + 10)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_incd(p, &d[0][0], n, i, j); + sum = + sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0], + n); + } + + return (sum); +} + +/** Wraps angle from (-M_PI,M_PI] + * @param[in] ang Angle in radians to wrap + * @return Wrapped angle from (-M_PI,M_PI] + */ +float inv_wrap_angle(float ang) +{ + if (ang > M_PI) + return ang - 2 * (float)M_PI; + else if (ang <= -(float)M_PI) + return ang + 2 * (float)M_PI; + else + return ang; +} + +/** Finds the minimum angle difference ang1-ang2 such that difference + * is between [-M_PI,M_PI] + * @param[in] ang1 + * @param[in] ang2 + * @return angle difference ang1-ang2 + */ +float inv_angle_diff(float ang1, float ang2) +{ + float d; + ang1 = inv_wrap_angle(ang1); + ang2 = inv_wrap_angle(ang2); + d = ang1 - ang2; + if (d > M_PI) + d -= 2 * (float)M_PI; + else if (d < -(float)M_PI) + d += 2 * (float)M_PI; + return d; +} |