1 /* 2 $License: 3 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 See included License.txt for License information. 5 $ 6 */ 7 #ifndef INVENSENSE_INV_MATH_FUNC_H__ 8 #define INVENSENSE_INV_MATH_FUNC_H__ 9 10 #include "mltypes.h" 11 12 #define GYRO_MAG_SQR_SHIFT 6 13 #define NUM_ROTATION_MATRIX_ELEMENTS (9) 14 #define ROT_MATRIX_SCALE_LONG (1073741824L) 15 #define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) 16 #define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ 17 ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) 18 #define SIGNM(k)((int)(k)&1?-1:1) 19 #define SIGNSET(x) ((x) ? -1 : +1) 20 21 #define INV_TWO_POWER_NEG_30 9.313225746154785e-010f 22 23 #ifdef __cplusplus 24 extern "C" { 25 #endif 26 27 typedef struct { 28 float state[4]; 29 float c[5]; 30 float input; 31 float output; 32 } inv_biquad_filter_t; 33 34 static inline float inv_q30_to_float(long q30) 35 { 36 return (float) q30 / ((float)(1L << 30)); 37 } 38 39 static inline double inv_q30_to_double(long q30) 40 { 41 return (double) q30 / ((double)(1L << 30)); 42 } 43 44 static inline float inv_q16_to_float(long q16) 45 { 46 return (float) q16 / (1L << 16); 47 } 48 49 static inline double inv_q16_to_double(long q16) 50 { 51 return (double) q16 / (1L << 16); 52 } 53 54 55 56 57 long inv_q29_mult(long a, long b); 58 long inv_q30_mult(long a, long b); 59 60 /* UMPL_ELIMINATE_64BIT Notes: 61 * An alternate implementation using float instead of long long accudoublemulators 62 * is provided for q29_mult and q30_mult. 63 * When long long accumulators are used and an alternate implementation is not 64 * available, we eliminate the entire function and header with a macro. 65 */ 66 #ifndef UMPL_ELIMINATE_64BIT 67 long inv_q30_div(long a, long b); 68 long inv_q_shift_mult(long a, long b, int shift); 69 #endif 70 71 void inv_q_mult(const long *q1, const long *q2, long *qProd); 72 void inv_q_add(long *q1, long *q2, long *qSum); 73 void inv_q_normalize(long *q); 74 void inv_q_invert(const long *q, long *qInverted); 75 void inv_q_multf(const float *q1, const float *q2, float *qProd); 76 void inv_q_addf(const float *q1, const float *q2, float *qSum); 77 void inv_q_normalizef(float *q); 78 void inv_q_norm4(float *q); 79 void inv_q_invertf(const float *q, float *qInverted); 80 void inv_quaternion_to_rotation(const long *quat, long *rot); 81 unsigned char *inv_int32_to_big8(long x, unsigned char *big8); 82 long inv_big8_to_int32(const unsigned char *big8); 83 short inv_big8_to_int16(const unsigned char *big8); 84 short inv_little8_to_int16(const unsigned char *little8); 85 unsigned char *inv_int16_to_big8(short x, unsigned char *big8); 86 float inv_matrix_det(float *p, int *n); 87 void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); 88 double inv_matrix_detd(double *p, int *n); 89 void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); 90 float inv_wrap_angle(float ang); 91 float inv_angle_diff(float ang1, float ang2); 92 void inv_quaternion_to_rotation_vector(const long *quat, long *rot); 93 unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); 94 void inv_convert_to_body(unsigned short orientation, const long *input, long *output); 95 void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); 96 void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); 97 void inv_q_rotate(const long *q, const long *in, long *out); 98 void inv_vector_normalize(long *vec, int length); 99 uint32_t inv_checksum(const unsigned char *str, int len); 100 float inv_compass_angle(const long *compass, const long *grav, 101 const float *quat); 102 unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); 103 104 static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) 105 { 106 return (long)((t1 - t2) / 1000000L); 107 } 108 109 double quaternion_to_rotation_angle(const long *quat); 110 double inv_vector_norm(const float *x); 111 112 void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); 113 float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); 114 void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); 115 void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); 116 117 void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut); 118 119 long inv_inverse_sqrt(long x0, int*rempow); 120 long inv_fast_sqrt(long x0); 121 long inv_one_over_x(long x0, int*pow); 122 int test_limits_and_scale(long *x0, int *pow); 123 int get_highest_bit_position(unsigned long *value); 124 int inv_compute_scalar_part(const long * inQuat, long* outQuat); 125 126 #ifdef __cplusplus 127 } 128 #endif 129 #endif // INVENSENSE_INV_MATH_FUNC_H__ 130
