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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
|
/**
* mlcompat! where symbols referenced by libinvensense_mpl.so go to die. :(
* They may one day rise again, e.g. inv_get_motion_state looks fun, but
* this helps keep track of where things are used and by whom are they used.
*/
#include "mlcompat.h"
#include "ml.h"
#include "mlMathFunc.h"
#include "mlmath.h"
inv_error_t inv_pressure_supervisor(void)
{
return INV_SUCCESS;
}
/**
* @brief inv_get_motion_state is used to determine if the device is in
* a 'motion' or 'no motion' state.
* inv_get_motion_state returns INV_MOTION of the device is moving,
* or INV_NO_MOTION if the device is not moving.
*
* @pre inv_dmp_open()
* @ifnot MPL_MF
* or inv_open_low_power_pedometer()
* or inv_eis_open_dmp()
* @endif
* and inv_dmp_start()
* must have been called.
*
* @return INV_SUCCESS if successful or Non-zero error code otherwise.
*/
int inv_get_motion_state(void)
{
return inv_obj.motion_state;
}
/**
* Performs one iteration of the filter, generating a new y(0)
* 1 / N / N \\
* y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length
* a(0) \k=0 \ k=1 //
*
* The filters A and B should be (sizeof(long) * state->length).
* The state variables x and y should be (sizeof(long) * (state->length - 1))
*
* The state variables x and y should be initialized prior to the first call
*
* @param state Contains the length of the filter, filter coefficients and
* filter state variables x and y.
* @param x New input into the filter.
*/
void inv_filter_long(struct filter_long *state, long x)
{
const long *b = state->b;
const long *a = state->a;
long length = state->length;
long long tmp;
int ii;
/* filter */
tmp = (long long)x *(b[0]);
for (ii = 0; ii < length - 1; ii++) {
tmp += ((long long)state->x[ii] * (long long)(b[ii + 1]));
}
for (ii = 0; ii < length - 1; ii++) {
tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1]));
}
tmp /= (long long)a[0];
/* Delay */
for (ii = length - 2; ii > 0; ii--) {
state->y[ii] = state->y[ii - 1];
state->x[ii] = state->x[ii - 1];
}
/* New values */
state->y[0] = (long)tmp;
state->x[0] = x;
}
void inv_q_multf(const float *q1, const float *q2, float *qProd)
{
qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
}
void inv_q_addf(float *q1, float *q2, float *qSum)
{
qSum[0] = q1[0] + q2[0];
qSum[1] = q1[1] + q2[1];
qSum[2] = q1[2] + q2[2];
qSum[3] = q1[3] + q2[3];
}
void inv_q_normalizef(float *q)
{
float normSF = 0;
float xHalf = 0;
normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
if (normSF < 2) {
xHalf = 0.5f * normSF;
normSF = normSF * (1.5f - xHalf * normSF * normSF);
normSF = normSF * (1.5f - xHalf * normSF * normSF);
normSF = normSF * (1.5f - xHalf * normSF * normSF);
normSF = normSF * (1.5f - xHalf * normSF * normSF);
q[0] *= normSF;
q[1] *= normSF;
q[2] *= normSF;
q[3] *= normSF;
} else {
q[0] = 1.0;
q[1] = 0.0;
q[2] = 0.0;
q[3] = 0.0;
}
normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
}
/** Performs a length 4 vector normalization with a square root.
* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero.
*/
void inv_q_norm4(float *q)
{
float mag;
mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
if (mag) {
q[0] /= mag;
q[1] /= mag;
q[2] /= mag;
q[3] /= mag;
} else {
q[0] = 1.f;
q[1] = 0.f;
q[2] = 0.f;
q[3] = 0.f;
}
}
void inv_q_invertf(const float *q, float *qInverted)
{
qInverted[0] = q[0];
qInverted[1] = -q[1];
qInverted[2] = -q[2];
qInverted[3] = -q[3];
}
void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
{
int k, l, i, j;
for (i = 0, k = 0; i < *n; i++, k++) {
for (j = 0, l = 0; j < *n; j++, l++) {
if (i == x)
i++;
if (j == y)
j++;
*(b + 10 * k + l) = *(a + 10 * i + j);
}
}
*n = *n - 1;
}
double inv_matrix_detd(double *p, int *n)
{
double d[10][10], sum = 0;
int i, j, m;
m = *n;
if (*n == 2)
return (*p ** (p + 11) - *(p + 1) ** (p + 10));
for (i = 0, j = 0; j < m; j++) {
*n = m;
inv_matrix_det_incd(p, &d[0][0], n, i, j);
sum =
sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0],
n);
}
return (sum);
}
inv_error_t inv_get_array(int dataSet, long *data)
{
switch (dataSet) {
case INV_GYROS:
return inv_get_gyro(data);
case INV_ACCELS:
return inv_get_accel(data);
case INV_TEMPERATURE:
return inv_get_temperature(data);
case INV_ROTATION_MATRIX:
return inv_get_rot_mat(data);
case INV_QUATERNION:
return inv_get_quaternion(data);
case INV_LINEAR_ACCELERATION:
return inv_get_linear_accel(data);
case INV_LINEAR_ACCELERATION_WORLD:
return inv_get_linear_accel_in_world(data);
case INV_GRAVITY:
return inv_get_gravity(data);
case INV_ANGULAR_VELOCITY:
return inv_get_angular_velocity(data);
case INV_EULER_ANGLES:
return inv_get_euler_angles(data);
case INV_EULER_ANGLES_X:
return inv_get_euler_angles_x(data);
case INV_EULER_ANGLES_Y:
return inv_get_euler_angles_y(data);
case INV_EULER_ANGLES_Z:
return inv_get_euler_angles_z(data);
case INV_GYRO_TEMP_SLOPE:
return inv_get_gyro_temp_slope(data);
case INV_GYRO_BIAS:
return inv_get_gyro_bias(data);
case INV_ACCEL_BIAS:
return inv_get_accel_bias(data);
case INV_MAG_BIAS:
return inv_get_mag_bias(data);
case INV_RAW_DATA:
return inv_get_gyro_and_accel_sensor(data);
case INV_MAG_RAW_DATA:
return inv_get_mag_raw_data(data);
case INV_MAGNETOMETER:
return inv_get_magnetometer(data);
case INV_PRESSURE:
return inv_get_pressure(data);
case INV_HEADING:
return inv_get_heading(data);
case INV_GYRO_CALIBRATION_MATRIX:
return inv_get_gyro_cal_matrix(data);
case INV_ACCEL_CALIBRATION_MATRIX:
return inv_get_accel_cal_matrix(data);
case INV_MAG_CALIBRATION_MATRIX:
return inv_get_mag_cal_matrix(data);
case INV_MAG_BIAS_ERROR:
return inv_get_mag_bias_error(data);
case INV_MAG_SCALE:
return inv_get_mag_scale(data);
case INV_LOCAL_FIELD:
return inv_get_local_field(data);
case INV_RELATIVE_QUATERNION:
return inv_get_relative_quaternion(data);
}
return INV_ERROR_INVALID_PARAMETER;
}
inv_error_t inv_get_float_array(int dataSet, float *data)
{
switch (dataSet) {
case INV_GYROS:
return inv_get_gyro_float(data);
case INV_ACCELS:
return inv_get_accel_float(data);
case INV_TEMPERATURE:
return inv_get_temperature_float(data);
case INV_ROTATION_MATRIX:
return inv_get_rot_mat_float(data);
case INV_QUATERNION:
return inv_get_quaternion_float(data);
case INV_LINEAR_ACCELERATION:
return inv_get_linear_accel_float(data);
case INV_LINEAR_ACCELERATION_WORLD:
return inv_get_linear_accel_in_world_float(data);
case INV_GRAVITY:
return inv_get_gravity_float(data);
case INV_ANGULAR_VELOCITY:
return inv_get_angular_velocity_float(data);
case INV_EULER_ANGLES:
return inv_get_euler_angles_float(data);
case INV_EULER_ANGLES_X:
return inv_get_euler_angles_x_float(data);
case INV_EULER_ANGLES_Y:
return inv_get_euler_angles_y_float(data);
case INV_EULER_ANGLES_Z:
return inv_get_euler_angles_z_float(data);
case INV_GYRO_TEMP_SLOPE:
return inv_get_gyro_temp_slope_float(data);
case INV_GYRO_BIAS:
return inv_get_gyro_bias_float(data);
case INV_ACCEL_BIAS:
return inv_get_accel_bias_float(data);
case INV_MAG_BIAS:
return inv_get_mag_bias_float(data);
case INV_RAW_DATA:
return inv_get_gyro_and_accel_sensor_float(data);
case INV_MAG_RAW_DATA:
return inv_get_mag_raw_data_float(data);
case INV_MAGNETOMETER:
return inv_get_magnetometer_float(data);
case INV_PRESSURE:
return inv_get_pressure_float(data);
case INV_HEADING:
return inv_get_heading_float(data);
case INV_GYRO_CALIBRATION_MATRIX:
return inv_get_gyro_cal_matrix_float(data);
case INV_ACCEL_CALIBRATION_MATRIX:
return inv_get_accel_cal_matrix_float(data);
case INV_MAG_CALIBRATION_MATRIX:
return inv_get_mag_cal_matrix_float(data);
case INV_MAG_BIAS_ERROR:
return inv_get_mag_bias_error_float(data);
case INV_MAG_SCALE:
return inv_get_mag_scale_float(data);
case INV_LOCAL_FIELD:
return inv_get_local_field_float(data);
case INV_RELATIVE_QUATERNION:
return inv_get_relative_quaternion_float(data);
}
return INV_ERROR_INVALID_PARAMETER;
}
|
