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