149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $License: 349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow See included License.txt for License information. 549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $ 649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef INVENSENSE_INV_MATH_FUNC_H__ 849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define INVENSENSE_INV_MATH_FUNC_H__ 949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mltypes.h" 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define GYRO_MAG_SQR_SHIFT 6 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define NUM_ROTATION_MATRIX_ELEMENTS (9) 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define ROT_MATRIX_SCALE_LONG (1073741824L) 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) 1649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ 1749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) 1849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define SIGNM(k)((int)(k)&1?-1:1) 1949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define SIGNSET(x) ((x) ? -1 : +1) 2049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f 2249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef __cplusplus 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowextern "C" { 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow typedef struct { 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float state[4]; 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float c[5]; 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float input; 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float output; 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } inv_biquad_filter_t; 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow static inline float inv_q30_to_float(long q30) 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (float) q30 / ((float)(1L << 30)); 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow static inline double inv_q30_to_double(long q30) 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (double) q30 / ((double)(1L << 30)); 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow static inline float inv_q16_to_float(long q16) 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (float) q16 / (1L << 16); 4749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow static inline double inv_q16_to_double(long q16) 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (double) q16 / (1L << 16); 5249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_q29_mult(long a, long b); 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_q30_mult(long a, long b); 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* UMPL_ELIMINATE_64BIT Notes: 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * An alternate implementation using float instead of long long accudoublemulators 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * is provided for q29_mult and q30_mult. 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * When long long accumulators are used and an alternate implementation is not 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * available, we eliminate the entire function and header with a macro. 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef UMPL_ELIMINATE_64BIT 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_q30_div(long a, long b); 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_q_shift_mult(long a, long b, int shift); 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_mult(const long *q1, const long *q2, long *qProd); 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_add(long *q1, long *q2, long *qSum); 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_normalize(long *q); 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_invert(const long *q, long *qInverted); 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_multf(const float *q1, const float *q2, float *qProd); 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_addf(const float *q1, const float *q2, float *qSum); 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_normalizef(float *q); 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_norm4(float *q); 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_invertf(const float *q, float *qInverted); 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_quaternion_to_rotation(const long *quat, long *rot); 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char *inv_int32_to_big8(long x, unsigned char *big8); 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_big8_to_int32(const unsigned char *big8); 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow short inv_big8_to_int16(const unsigned char *big8); 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow short inv_little8_to_int16(const unsigned char *little8); 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char *inv_int16_to_big8(short x, unsigned char *big8); 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float inv_matrix_det(float *p, int *n); 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow double inv_matrix_detd(double *p, int *n); 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); 9049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float inv_wrap_angle(float ang); 9149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float inv_angle_diff(float ang1, float ang2); 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_quaternion_to_rotation_vector(const long *quat, long *rot); 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); 9449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_convert_to_body(unsigned short orientation, const long *input, long *output); 9549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); 9649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); 9749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_q_rotate(const long *q, const long *in, long *out); 9849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_vector_normalize(long *vec, int length); 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t inv_checksum(const unsigned char *str, int len); 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float inv_compass_angle(const long *compass, const long *grav, 10149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow const float *quat); 10249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); 10349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 10449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) 10549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 10649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (long)((t1 - t2) / 1000000L); 10749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 10949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow double quaternion_to_rotation_angle(const long *quat); 11049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow double inv_vector_norm(const float *x); 11149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut); 11849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_inverse_sqrt(long x0, int*rempow); 12049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_fast_sqrt(long x0); 12149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long inv_one_over_x(long x0, int*pow); 12249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int test_limits_and_scale(long *x0, int *pow); 12349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int get_highest_bit_position(unsigned long *value); 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int inv_compute_scalar_part(const long * inQuat, long* outQuat); 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef __cplusplus 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 12949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif // INVENSENSE_INV_MATH_FUNC_H__ 130