17494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/* 27494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $License: 37494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 47494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall See included License.txt for License information. 57494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $ 67494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 77494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifndef INVENSENSE_INV_MATH_FUNC_H__ 87494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define INVENSENSE_INV_MATH_FUNC_H__ 97494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "mltypes.h" 117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define GYRO_MAG_SQR_SHIFT 6 137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define NUM_ROTATION_MATRIX_ELEMENTS (9) 147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define ROT_MATRIX_SCALE_LONG (1073741824L) 157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) 167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ 177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) 187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define SIGNM(k)((int)(k)&1?-1:1) 197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define SIGNSET(x) ((x) ? -1 : +1) 207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f 227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef __cplusplus 247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallextern "C" { 257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 2733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall typedef struct { 2833ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float state[4]; 2933ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float c[5]; 3033ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float input; 3133ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float output; 3233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall } inv_biquad_filter_t; 3333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall static inline float inv_q30_to_float(long q30) 357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall { 367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (float) q30 / ((float)(1L << 30)); 377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall static inline double inv_q30_to_double(long q30) 407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall { 417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (double) q30 / ((double)(1L << 30)); 427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall static inline float inv_q16_to_float(long q16) 457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall { 467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (float) q16 / (1L << 16); 477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall static inline double inv_q16_to_double(long q16) 507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall { 517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (double) q16 / (1L << 16); 527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long inv_q29_mult(long a, long b); 587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long inv_q30_mult(long a, long b); 597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall /* UMPL_ELIMINATE_64BIT Notes: 617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * An alternate implementation using float instead of long long accudoublemulators 627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * is provided for q29_mult and q30_mult. 637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * When long long accumulators are used and an alternate implementation is not 647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * available, we eliminate the entire function and header with a macro. 657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifndef UMPL_ELIMINATE_64BIT 677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long inv_q30_div(long a, long b); 687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long inv_q_shift_mult(long a, long b, int shift); 697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_mult(const long *q1, const long *q2, long *qProd); 727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_add(long *q1, long *q2, long *qSum); 737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_normalize(long *q); 747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_invert(const long *q, long *qInverted); 757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_multf(const float *q1, const float *q2, float *qProd); 767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_addf(const float *q1, const float *q2, float *qSum); 777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_normalizef(float *q); 787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_norm4(float *q); 797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_invertf(const float *q, float *qInverted); 807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_quaternion_to_rotation(const long *quat, long *rot); 817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned char *inv_int32_to_big8(long x, unsigned char *big8); 827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long inv_big8_to_int32(const unsigned char *big8); 837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall short inv_big8_to_int16(const unsigned char *big8); 847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall short inv_little8_to_int16(const unsigned char *little8); 857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned char *inv_int16_to_big8(short x, unsigned char *big8); 867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float inv_matrix_det(float *p, int *n); 877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); 887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall double inv_matrix_detd(double *p, int *n); 897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); 907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float inv_wrap_angle(float ang); 917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float inv_angle_diff(float ang1, float ang2); 927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_quaternion_to_rotation_vector(const long *quat, long *rot); 937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); 947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_convert_to_body(unsigned short orientation, const long *input, long *output); 957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); 967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); 977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_q_rotate(const long *q, const long *in, long *out); 987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall void inv_vector_normalize(long *vec, int length); 997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall uint32_t inv_checksum(const unsigned char *str, int len); 1007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall float inv_compass_angle(const long *compass, const long *grav, 1017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall const float *quat); 1027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); 1037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) 1057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall { 1067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return (long)((t1 - t2) / 1000000L); 1077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall double quaternion_to_rotation_angle(const long *quat); 1107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall double inv_vector_norm(const float *x); 1117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 11233ce91b37062fa63af192f5643de93f3beebe854JP Abgrall void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); 11333ce91b37062fa63af192f5643de93f3beebe854JP Abgrall float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); 11433ce91b37062fa63af192f5643de93f3beebe854JP Abgrall void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); 11533ce91b37062fa63af192f5643de93f3beebe854JP Abgrall void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); 11633ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 1177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#ifdef __cplusplus 1187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif 1207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#endif // INVENSENSE_INV_MATH_FUNC_H__ 121