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
24extern "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