1 /*
2 $License:
3 Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16 $
17 */
18 /******************************************************************************
19 *
20 * $Id: mlarray.c 5085 2011-04-08 22:25:14Z phickey $
21 *
22 *****************************************************************************/
23
24 /**
25 * @defgroup ML
26 * @{
27 * @file mlarray.c
28 * @brief APIs to read different data sets from FIFO.
29 */
30
31 /* ------------------ */
32 /* - Include Files. - */
33 /* ------------------ */
34 #include "ml.h"
35 #include "mltypes.h"
36 #include "mlinclude.h"
37 #include "mlMathFunc.h"
38 #include "mlmath.h"
39 #include "mlstates.h"
40 #include "mlFIFO.h"
41 #include "mlsupervisor.h"
42 #include "mldl.h"
43 #include "dmpKey.h"
44 #include "compass.h"
45
46 /**
47 * @brief inv_get_gyro is used to get the most recent gyroscope measurement.
48 * The argument array elements are ordered X,Y,Z.
49 * The values are scaled at 1 dps = 2^16 LSBs.
50 *
51 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
52 * must have been called.
53 *
54 * @param data
55 * A pointer to an array to be passed back to the user.
56 * <b>Must be 3 cells long </b>.
57 *
58 * @return Zero if the command is successful; an ML error code otherwise.
59 */
60
61 /* inv_get_gyro implemented in mlFIFO.c */
62
63 /**
64 * @brief inv_get_accel is used to get the most recent
65 * accelerometer measurement.
66 * The argument array elements are ordered X,Y,Z.
67 * The values are scaled in units of g (gravity),
68 * where 1 g = 2^16 LSBs.
69 *
70 *
71 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
72 * must have been called.
73 *
74 * @param data
75 * A pointer to an array to be passed back to the user.
76 * <b>Must be 3 cells long </b>.
77 *
78 * @return Zero if the command is successful; an ML error code otherwise.
79 */
80 /* inv_get_accel implemented in mlFIFO.c */
81
82 /**
83 * @brief inv_get_temperature is used to get the most recent
84 * temperature measurement.
85 * The argument array should only have one element.
86 * The value is in units of deg C (degrees Celsius), where
87 * 2^16 LSBs = 1 deg C.
88 *
89 *
90 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
91 * must have been called.
92 *
93 * @param data
94 * A pointer to the data to be passed back to the user.
95 *
96 * @return Zero if the command is successful; an ML error code otherwise.
97 */
98 /* inv_get_temperature implemented in mlFIFO.c */
99
100 /**
101 * @brief inv_get_rot_mat is used to get the rotation matrix
102 * representation of the current sensor fusion solution.
103 * The array format will be R11, R12, R13, R21, R22, R23, R31, R32,
104 * R33, representing the matrix:
105 * <center>R11 R12 R13</center>
106 * <center>R21 R22 R23</center>
107 * <center>R31 R32 R33</center>
108 * Values are scaled, where 1.0 = 2^30 LSBs.
109 *
110 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
111 * must have been called.
112 *
113 * @param data
114 * A pointer to an array to be passed back to the user.
115 * <b>Must be 9 cells long</b>.
116 *
117 * @return Zero if the command is successful; an ML error code otherwise.
118 */
inv_get_rot_mat(long * data)119 inv_error_t inv_get_rot_mat(long *data)
120 {
121 inv_error_t result = INV_SUCCESS;
122 long qdata[4];
123 if (inv_get_state() < INV_STATE_DMP_OPENED)
124 return INV_ERROR_SM_IMPROPER_STATE;
125
126 if (NULL == data) {
127 return INV_ERROR_INVALID_PARAMETER;
128 }
129
130 inv_get_quaternion(qdata);
131 inv_quaternion_to_rotation(qdata, data);
132
133 return result;
134 }
135
136 /**
137 * @brief inv_get_quaternion is used to get the quaternion representation
138 * of the current sensor fusion solution.
139 * The values are scaled where 1.0 = 2^30 LSBs.
140 *
141 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
142 * must have been called.
143 *
144 * @param data
145 * A pointer to an array to be passed back to the user.
146 * <b>Must be 4 cells long </b>.
147 *
148 * @return INV_SUCCESS if the command is successful; an ML error code otherwise.
149 */
150 /* inv_get_quaternion implemented in mlFIFO.c */
151
152 /**
153 * @brief inv_get_linear_accel is used to get an estimate of linear
154 * acceleration, based on the most recent accelerometer measurement
155 * and sensor fusion solution.
156 * The values are scaled where 1 g (gravity) = 2^16 LSBs.
157 *
158 *
159 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
160 * must have been called.
161 *
162 * @param data
163 * A pointer to an array to be passed back to the user.
164 * <b>Must be 3 cells long </b>.
165 *
166 * @return Zero if the command is successful; an ML error code otherwise.
167 */
168 /* inv_get_linear_accel implemented in mlFIFO.c */
169
170 /**
171 * @brief inv_get_linear_accel_in_world is used to get an estimate of
172 * linear acceleration, in the world frame, based on the most
173 * recent accelerometer measurement and sensor fusion solution.
174 * The values are scaled where 1 g (gravity) = 2^16 LSBs.
175 *
176 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
177 * must have been called.
178 *
179 * @param data
180 * A pointer to an array to be passed back to the user.
181 * <b>Must be 3 cells long</b>.
182 *
183 * @return Zero if the command is successful; an ML error code otherwise.
184 */
185 /* inv_get_linear_accel_in_world implemented in mlFIFO.c */
186
187 /**
188 * @brief inv_get_gravity is used to get an estimate of the body frame
189 * gravity vector, based on the most recent sensor fusion solution.
190 *
191 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
192 * must have been called.
193 *
194 * @param data
195 * A pointer to an array to be passed back to the user.
196 * <b>Must be 3 cells long</b>.
197 *
198 * @return Zero if the command is successful; an ML error code otherwise.
199 */
200 /* inv_get_gravity implemented in mlFIFO.c */
201
202 /**
203 * @internal
204 * @brief inv_get_angular_velocity is used to get an estimate of the body
205 * frame angular velocity, which is computed from the current and
206 * previous sensor fusion solutions.
207 *
208 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
209 * must have been called.
210 *
211 * @param data
212 * A pointer to an array to be passed back to the user.
213 * <b>Must be 3 cells long </b>.
214 *
215 * @return Zero if the command is successful; an ML error code otherwise.
216 */
inv_get_angular_velocity(long * data)217 inv_error_t inv_get_angular_velocity(long *data)
218 {
219
220 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
221 /* not implemented. old (invalid) implementation:
222 if ( inv_get_state() < INV_STATE_DMP_OPENED )
223 return INV_ERROR_SM_IMPROPER_STATE;
224
225 if (NULL == data) {
226 return INV_ERROR_INVALID_PARAMETER;
227 }
228 data[0] = inv_obj.ang_v_body[0];
229 data[1] = inv_obj.ang_v_body[1];
230 data[2] = inv_obj.ang_v_body[2];
231
232 return result;
233 */
234 }
235
236 /**
237 * @brief inv_get_euler_angles is used to get the Euler angle representation
238 * of the current sensor fusion solution.
239 * Euler angles may follow various conventions. This function is equivelant
240 * to inv_get_euler_angles_x, refer to inv_get_euler_angles_x for more
241 * information.
242 *
243 *
244 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
245 * must have been called.
246 *
247 * @param data
248 * A pointer to an array to be passed back to the user.
249 * <b>Must be 3 cells long </b>.
250 *
251 * @return Zero if the command is successful; an ML error code otherwise.
252 */
inv_get_euler_angles(long * data)253 inv_error_t inv_get_euler_angles(long *data)
254 {
255 return inv_get_euler_angles_x(data);
256 }
257
258 /**
259 * @brief inv_get_euler_angles_x is used to get the Euler angle representation
260 * of the current sensor fusion solution.
261 * Euler angles are returned according to the X convention.
262 * This is typically the convention used for mobile devices where the X
263 * axis is the width of the screen, Y axis is the height, and Z the
264 * depth. In this case roll is defined as the rotation around the X
265 * axis of the device.
266 * <TABLE>
267 * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
268 * <TR><TD> 0 </TD><TD>Roll </TD><TD>X axis </TD></TR>
269 * <TR><TD> 1 </TD><TD>Pitch </TD><TD>Y axis </TD></TR>
270 * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
271 * </TABLE>
272 *
273 * Values are scaled where 1.0 = 2^16.
274 *
275 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
276 * must have been called.
277 *
278 * @param data
279 * A pointer to an array to be passed back to the user.
280 * <b>Must be 3 cells long </b>.
281 *
282 * @return Zero if the command is successful; an ML error code otherwise.
283 */
inv_get_euler_angles_x(long * data)284 inv_error_t inv_get_euler_angles_x(long *data)
285 {
286 inv_error_t result = INV_SUCCESS;
287 float rotMatrix[9];
288 float tmp;
289 if (inv_get_state() < INV_STATE_DMP_OPENED)
290 return INV_ERROR_SM_IMPROPER_STATE;
291
292 if (NULL == data) {
293 return INV_ERROR_INVALID_PARAMETER;
294 }
295
296 result = inv_get_rot_mat_float(rotMatrix);
297 tmp = rotMatrix[6];
298 if (tmp > 1.0f) {
299 tmp = 1.0f;
300 }
301 if (tmp < -1.0f) {
302 tmp = -1.0f;
303 }
304 data[0] =
305 (long)((float)
306 (atan2f(rotMatrix[7], rotMatrix[8]) * 57.29577951308) *
307 65536L);
308 data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
309 data[2] =
310 (long)((float)
311 (atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308) *
312 65536L);
313 return result;
314 }
315
316 /**
317 * @brief inv_get_euler_angles_y is used to get the Euler angle representation
318 * of the current sensor fusion solution.
319 * Euler angles are returned according to the Y convention.
320 * This convention is typically used in augmented reality applications,
321 * where roll is defined as the rotation around the axis along the
322 * height of the screen of a mobile device, namely the Y axis.
323 * <TABLE>
324 * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
325 * <TR><TD> 0 </TD><TD>Roll </TD><TD>Y axis </TD></TR>
326 * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
327 * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
328 * </TABLE>
329 *
330 * Values are scaled where 1.0 = 2^16.
331 *
332 *
333 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
334 * must have been called.
335 *
336 * @param data
337 * A pointer to an array to be passed back to the user.
338 * <b>Must be 3 cells long</b>.
339 *
340 * @return Zero if the command is successful; an ML error code otherwise.
341 */
inv_get_euler_angles_y(long * data)342 inv_error_t inv_get_euler_angles_y(long *data)
343 {
344 inv_error_t result = INV_SUCCESS;
345 float rotMatrix[9];
346 float tmp;
347 if (inv_get_state() < INV_STATE_DMP_OPENED)
348 return INV_ERROR_SM_IMPROPER_STATE;
349
350 if (NULL == data) {
351 return INV_ERROR_INVALID_PARAMETER;
352 }
353
354 result = inv_get_rot_mat_float(rotMatrix);
355 tmp = rotMatrix[7];
356 if (tmp > 1.0f) {
357 tmp = 1.0f;
358 }
359 if (tmp < -1.0f) {
360 tmp = -1.0f;
361 }
362 data[0] =
363 (long)((float)
364 (atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308f) *
365 65536L);
366 data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
367 data[2] =
368 (long)((float)
369 (atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308f) *
370 65536L);
371 return result;
372 }
373
374 /** @brief inv_get_euler_angles_z is used to get the Euler angle representation
375 * of the current sensor fusion solution.
376 * This convention is mostly used in application involving the use
377 * of a camera, typically placed on the back of a mobile device, that
378 * is along the Z axis. In this convention roll is defined as the
379 * rotation around the Z axis.
380 * Euler angles are returned according to the Y convention.
381 * <TABLE>
382 * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
383 * <TR><TD> 0 </TD><TD>Roll </TD><TD>Z axis </TD></TR>
384 * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
385 * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Y axis </TD></TR>
386 * </TABLE>
387 *
388 * Values are scaled where 1.0 = 2^16.
389 *
390 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
391 * must have been called.
392 *
393 * @param data
394 * A pointer to an array to be passed back to the user.
395 * <b>Must be 3 cells long</b>.
396 *
397 * @return INV_SUCCESS if the command is successful; an error code otherwise.
398 */
399
inv_get_euler_angles_z(long * data)400 inv_error_t inv_get_euler_angles_z(long *data)
401 {
402 inv_error_t result = INV_SUCCESS;
403 float rotMatrix[9];
404 float tmp;
405 if (inv_get_state() < INV_STATE_DMP_OPENED)
406 return INV_ERROR_SM_IMPROPER_STATE;
407
408 if (NULL == data) {
409 return INV_ERROR_INVALID_PARAMETER;
410 }
411
412 result = inv_get_rot_mat_float(rotMatrix);
413 tmp = rotMatrix[8];
414 if (tmp > 1.0f) {
415 tmp = 1.0f;
416 }
417 if (tmp < -1.0f) {
418 tmp = -1.0f;
419 }
420 data[0] =
421 (long)((float)
422 (atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308) *
423 65536L);
424 data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
425 data[2] =
426 (long)((float)
427 (atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308) *
428 65536L);
429 return result;
430 }
431
432 /**
433 * @brief inv_get_gyro_temp_slope is used to get is used to get the temperature
434 * compensation algorithm's estimate of the gyroscope bias temperature
435 * coefficient.
436 * The argument array elements are ordered X,Y,Z.
437 * Values are in units of dps per deg C (degrees per second per degree
438 * Celcius). Values are scaled so that 1 dps per deg C = 2^16 LSBs.
439 * Please refer to the provided "9-Axis Sensor Fusion
440 * Application Note" document.
441 *
442 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
443 * must have been called.
444 *
445 * @param data
446 * A pointer to an array to be passed back to the user.
447 * <b>Must be 3 cells long </b>.
448 *
449 * @return Zero if the command is successful; an ML error code otherwise.
450 */
inv_get_gyro_temp_slope(long * data)451 inv_error_t inv_get_gyro_temp_slope(long *data)
452 {
453 inv_error_t result = INV_SUCCESS;
454 if (inv_get_state() < INV_STATE_DMP_OPENED)
455 return INV_ERROR_SM_IMPROPER_STATE;
456
457 if (NULL == data) {
458 return INV_ERROR_INVALID_PARAMETER;
459 }
460 if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
461 data[0] = (long)(inv_obj.x_gyro_coef[1] * 65536.0f);
462 data[1] = (long)(inv_obj.y_gyro_coef[1] * 65536.0f);
463 data[2] = (long)(inv_obj.z_gyro_coef[1] * 65536.0f);
464 } else {
465 data[0] = inv_obj.temp_slope[0];
466 data[1] = inv_obj.temp_slope[1];
467 data[2] = inv_obj.temp_slope[2];
468 }
469 return result;
470 }
471
472 /**
473 * @brief inv_get_gyro_bias is used to get the gyroscope biases.
474 * The argument array elements are ordered X,Y,Z.
475 * The values are scaled such that 1 dps = 2^16 LSBs.
476 *
477 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
478 * must have been called.
479 *
480 * @param data
481 * A pointer to an array to be passed back to the user.
482 * <b>Must be 3 cells long</b>.
483 *
484 * @return Zero if the command is successful; an ML error code otherwise.
485 */
inv_get_gyro_bias(long * data)486 inv_error_t inv_get_gyro_bias(long *data)
487 {
488 inv_error_t result = INV_SUCCESS;
489 if (inv_get_state() < INV_STATE_DMP_OPENED)
490 return INV_ERROR_SM_IMPROPER_STATE;
491
492 if (NULL == data) {
493 return INV_ERROR_INVALID_PARAMETER;
494 }
495 data[0] = inv_obj.gyro_bias[0];
496 data[1] = inv_obj.gyro_bias[1];
497 data[2] = inv_obj.gyro_bias[2];
498
499 return result;
500 }
501
502 /**
503 * @brief inv_get_accel_bias is used to get the accelerometer baises.
504 * The argument array elements are ordered X,Y,Z.
505 * The values are scaled such that 1 g (gravity) = 2^16 LSBs.
506 *
507 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
508 * must have been called.
509 *
510 * @param data
511 * A pointer to an array to be passed back to the user.
512 * <b>Must be 3 cells long </b>.
513 *
514 * @return Zero if the command is successful; an ML error code otherwise.
515 */
inv_get_accel_bias(long * data)516 inv_error_t inv_get_accel_bias(long *data)
517 {
518 inv_error_t result = INV_SUCCESS;
519 if (inv_get_state() < INV_STATE_DMP_OPENED)
520 return INV_ERROR_SM_IMPROPER_STATE;
521
522 if (NULL == data) {
523 return INV_ERROR_INVALID_PARAMETER;
524 }
525 data[0] = inv_obj.accel_bias[0];
526 data[1] = inv_obj.accel_bias[1];
527 data[2] = inv_obj.accel_bias[2];
528
529 return result;
530 }
531
532 /**
533 * @cond MPL
534 * @brief inv_get_mag_bias is used to get Magnetometer Bias
535 *
536 *
537 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
538 * must have been called.
539 *
540 * @param data
541 * A pointer to an array to be passed back to the user.
542 * <b>Must be 3 cells long </b>.
543 *
544 * @return Zero if the command is successful; an ML error code otherwise.
545 * @endcond
546 */
inv_get_mag_bias(long * data)547 inv_error_t inv_get_mag_bias(long *data)
548 {
549 inv_error_t result = INV_SUCCESS;
550 if (inv_get_state() < INV_STATE_DMP_OPENED)
551 return INV_ERROR_SM_IMPROPER_STATE;
552
553 if (NULL == data) {
554 return INV_ERROR_INVALID_PARAMETER;
555 }
556 data[0] =
557 inv_obj.compass_bias[0] +
558 (long)((long long)inv_obj.init_compass_bias[0] * inv_obj.compass_sens /
559 16384);
560 data[1] =
561 inv_obj.compass_bias[1] +
562 (long)((long long)inv_obj.init_compass_bias[1] * inv_obj.compass_sens /
563 16384);
564 data[2] =
565 inv_obj.compass_bias[2] +
566 (long)((long long)inv_obj.init_compass_bias[2] * inv_obj.compass_sens /
567 16384);
568
569 return result;
570 }
571
572 /**
573 * @brief inv_get_gyro_and_accel_sensor is used to get the most recent set of all sensor data.
574 * The argument array elements are ordered gyroscope X,Y, and Z,
575 * accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
576 * \if UMPL The magnetometer elements are not populated in UMPL. \endif
577 * The gyroscope and accelerometer data is not scaled or offset, it is
578 * copied directly from the sensor registers.
579 * In the case of accelerometers with 8-bit output resolution, the data
580 * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
581 * full scale range
582 *
583 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
584 * must have been called.
585 *
586 * @param data
587 * A pointer to an array to be passed back to the user.
588 * <b>Must be 9 cells long</b>.
589 *
590 * @return Zero if the command is successful; an ML error code otherwise.
591 */
592 /* inv_get_gyro_and_accel_sensor implemented in mlFIFO.c */
593
594 /**
595 * @cond MPL
596 * @brief inv_get_mag_raw_data is used to get Raw magnetometer data.
597 *
598 *
599 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
600 * must have been called.
601 *
602 * @param data
603 * A pointer to an array to be passed back to the user.
604 * <b>Must be 3 cells long </b>.
605 *
606 * @return Zero if the command is successful; an ML error code otherwise.
607 * @endcond
608 */
inv_get_mag_raw_data(long * data)609 inv_error_t inv_get_mag_raw_data(long *data)
610 {
611 inv_error_t result = INV_SUCCESS;
612 if (inv_get_state() < INV_STATE_DMP_OPENED)
613 return INV_ERROR_SM_IMPROPER_STATE;
614
615 if (NULL == data) {
616 return INV_ERROR_INVALID_PARAMETER;
617 }
618
619 data[0] = inv_obj.compass_sensor_data[0];
620 data[1] = inv_obj.compass_sensor_data[1];
621 data[2] = inv_obj.compass_sensor_data[2];
622
623 return result;
624 }
625
626 /**
627 * @cond MPL
628 * @brief inv_get_magnetometer is used to get magnetometer data.
629 *
630 *
631 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
632 * must have been called.
633 *
634 * @param data
635 * A pointer to an array to be passed back to the user.
636 * <b>Must be 3 cells long</b>.
637 *
638 * @return Zero if the command is successful; an ML error code otherwise.
639 * @endcond
640 */
inv_get_magnetometer(long * data)641 inv_error_t inv_get_magnetometer(long *data)
642 {
643 inv_error_t result = INV_SUCCESS;
644 if (inv_get_state() < INV_STATE_DMP_OPENED)
645 return INV_ERROR_SM_IMPROPER_STATE;
646
647 if (NULL == data) {
648 return INV_ERROR_INVALID_PARAMETER;
649 }
650
651 data[0] = inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0];
652 data[1] = inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1];
653 data[2] = inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2];
654
655 return result;
656 }
657
658 /**
659 * @cond MPL
660 * @brief inv_get_pressure is used to get Pressure data.
661 *
662 *
663 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
664 * must have been called.
665 *
666 * @param data
667 * A pointer to data to be passed back to the user.
668 *
669 * @return Zero if the command is successful; an ML error code otherwise.
670 * @endcond
671 */
inv_get_pressure(long * data)672 inv_error_t inv_get_pressure(long *data)
673 {
674 inv_error_t result = INV_SUCCESS;
675 if (inv_get_state() < INV_STATE_DMP_OPENED)
676 return INV_ERROR_SM_IMPROPER_STATE;
677
678 if (NULL == data) {
679 return INV_ERROR_INVALID_PARAMETER;
680 }
681
682 data[0] = inv_obj.pressure;
683
684 return result;
685 }
686
687 /**
688 * @cond MPL
689 * @brief inv_get_heading is used to get heading from Rotation Matrix.
690 *
691 *
692 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
693 * must have been called.
694 *
695 * @param data
696 * A pointer to data to be passed back to the user.
697 *
698 * @return Zero if the command is successful; an ML error code otherwise.
699 * @endcond
700 */
701
inv_get_heading(long * data)702 inv_error_t inv_get_heading(long *data)
703 {
704 inv_error_t result = INV_SUCCESS;
705 float rotMatrix[9];
706 float tmp;
707 if (inv_get_state() < INV_STATE_DMP_OPENED)
708 return INV_ERROR_SM_IMPROPER_STATE;
709
710 if (NULL == data) {
711 return INV_ERROR_INVALID_PARAMETER;
712 }
713 result = inv_get_rot_mat_float(rotMatrix);
714 if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
715 tmp =
716 (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
717 90.0f);
718 } else {
719 tmp =
720 (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
721 90.0f);
722 }
723 if (tmp < 0) {
724 tmp += 360.0f;
725 }
726 data[0] = (long)((360 - tmp) * 65536.0f);
727
728 return result;
729 }
730
731 /**
732 * @brief inv_get_gyro_cal_matrix is used to get the gyroscope
733 * calibration matrix. The gyroscope calibration matrix defines the relationship
734 * between the gyroscope sensor axes and the sensor fusion solution axes.
735 * Calibration matrix data members will have a value of 1, 0, or -1.
736 * The matrix has members
737 * <center>C11 C12 C13</center>
738 * <center>C21 C22 C23</center>
739 * <center>C31 C32 C33</center>
740 * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
741 *
742 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
743 * must have been called.
744 *
745 * @param data
746 * A pointer to an array to be passed back to the user.
747 * <b>Must be 9 cells long</b>.
748 *
749 * @return Zero if the command is successful; an ML error code otherwise.
750 */
inv_get_gyro_cal_matrix(long * data)751 inv_error_t inv_get_gyro_cal_matrix(long *data)
752 {
753 inv_error_t result = INV_SUCCESS;
754 if (inv_get_state() < INV_STATE_DMP_OPENED)
755 return INV_ERROR_SM_IMPROPER_STATE;
756
757 if (NULL == data) {
758 return INV_ERROR_INVALID_PARAMETER;
759 }
760
761 data[0] = inv_obj.gyro_cal[0];
762 data[1] = inv_obj.gyro_cal[1];
763 data[2] = inv_obj.gyro_cal[2];
764 data[3] = inv_obj.gyro_cal[3];
765 data[4] = inv_obj.gyro_cal[4];
766 data[5] = inv_obj.gyro_cal[5];
767 data[6] = inv_obj.gyro_cal[6];
768 data[7] = inv_obj.gyro_cal[7];
769 data[8] = inv_obj.gyro_cal[8];
770
771 return result;
772 }
773
774 /**
775 * @brief inv_get_accel_cal_matrix is used to get the accelerometer
776 * calibration matrix.
777 * Calibration matrix data members will have a value of 1, 0, or -1.
778 * The matrix has members
779 * <center>C11 C12 C13</center>
780 * <center>C21 C22 C23</center>
781 * <center>C31 C32 C33</center>
782 * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
783 *
784 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
785 *
786 *
787 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
788 * must have been called.
789 *
790 * @param data
791 * A pointer to an array to be passed back to the user.
792 * <b>Must be 9 cells long</b>.
793 *
794 * @return Zero if the command is successful; an ML error code otherwise.
795 */
inv_get_accel_cal_matrix(long * data)796 inv_error_t inv_get_accel_cal_matrix(long *data)
797 {
798 inv_error_t result = INV_SUCCESS;
799 if (inv_get_state() < INV_STATE_DMP_OPENED)
800 return INV_ERROR_SM_IMPROPER_STATE;
801
802 if (NULL == data) {
803 return INV_ERROR_INVALID_PARAMETER;
804 }
805
806 data[0] = inv_obj.accel_cal[0];
807 data[1] = inv_obj.accel_cal[1];
808 data[2] = inv_obj.accel_cal[2];
809 data[3] = inv_obj.accel_cal[3];
810 data[4] = inv_obj.accel_cal[4];
811 data[5] = inv_obj.accel_cal[5];
812 data[6] = inv_obj.accel_cal[6];
813 data[7] = inv_obj.accel_cal[7];
814 data[8] = inv_obj.accel_cal[8];
815
816 return result;
817 }
818
819 /**
820 * @cond MPL
821 * @brief inv_get_mag_cal_matrix is used to get magnetometer calibration matrix.
822 *
823 *
824 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
825 * must have been called.
826 *
827 * @param data
828 * A pointer to an array to be passed back to the user.
829 * <b>Must be 9 cells long at least</b>.
830 *
831 * @return Zero if the command is successful; an ML error code otherwise.
832 * @endcond
833 */
inv_get_mag_cal_matrix(long * data)834 inv_error_t inv_get_mag_cal_matrix(long *data)
835 {
836 inv_error_t result = INV_SUCCESS;
837 if (inv_get_state() < INV_STATE_DMP_OPENED)
838 return INV_ERROR_SM_IMPROPER_STATE;
839
840 if (NULL == data) {
841 return INV_ERROR_INVALID_PARAMETER;
842 }
843
844 data[0] = inv_obj.compass_cal[0];
845 data[1] = inv_obj.compass_cal[1];
846 data[2] = inv_obj.compass_cal[2];
847 data[3] = inv_obj.compass_cal[3];
848 data[4] = inv_obj.compass_cal[4];
849 data[5] = inv_obj.compass_cal[5];
850 data[6] = inv_obj.compass_cal[6];
851 data[7] = inv_obj.compass_cal[7];
852 data[8] = inv_obj.compass_cal[8];
853
854 return result;
855 }
856
857 /**
858 * @cond MPL
859 * @brief inv_get_mag_bias_error is used to get magnetometer Bias error.
860 *
861 *
862 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
863 * must have been called.
864 *
865 * @param data
866 * A pointer to an array to be passed back to the user.
867 * <b>Must be 3 cells long at least</b>.
868 *
869 * @return Zero if the command is successful; an ML error code otherwise.
870 * @endcond
871 */
inv_get_mag_bias_error(long * data)872 inv_error_t inv_get_mag_bias_error(long *data)
873 {
874 inv_error_t result = INV_SUCCESS;
875 if (inv_get_state() < INV_STATE_DMP_OPENED)
876 return INV_ERROR_SM_IMPROPER_STATE;
877
878 if (NULL == data) {
879 return INV_ERROR_INVALID_PARAMETER;
880 }
881 if (inv_obj.large_field == 0) {
882 data[0] = inv_obj.compass_bias_error[0];
883 data[1] = inv_obj.compass_bias_error[1];
884 data[2] = inv_obj.compass_bias_error[2];
885 } else {
886 data[0] = P_INIT;
887 data[1] = P_INIT;
888 data[2] = P_INIT;
889 }
890
891 return result;
892 }
893
894 /**
895 * @cond MPL
896 * @brief inv_get_mag_scale is used to get magnetometer scale.
897 *
898 *
899 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
900 * must have been called.
901 *
902 * @param data
903 * A pointer to an array to be passed back to the user.
904 * <b>Must be 3 cells long at least</b>.
905 *
906 * @return Zero if the command is successful; an ML error code otherwise.
907 * @endcond
908 */
inv_get_mag_scale(long * data)909 inv_error_t inv_get_mag_scale(long *data)
910 {
911 inv_error_t result = INV_SUCCESS;
912 if (inv_get_state() < INV_STATE_DMP_OPENED)
913 return INV_ERROR_SM_IMPROPER_STATE;
914
915 if (NULL == data) {
916 return INV_ERROR_INVALID_PARAMETER;
917 }
918
919 data[0] = inv_obj.compass_scale[0];
920 data[1] = inv_obj.compass_scale[1];
921 data[2] = inv_obj.compass_scale[2];
922
923 return result;
924 }
925
926 /**
927 * @cond MPL
928 * @brief inv_get_local_field is used to get local magnetic field data.
929 *
930 *
931 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
932 * must have been called.
933 *
934 * @param data
935 * A pointer to an array to be passed back to the user.
936 * <b>Must be 3 cells long at least</b>.
937 *
938 * @return Zero if the command is successful; an ML error code otherwise.
939 * @endcond
940 */
inv_get_local_field(long * data)941 inv_error_t inv_get_local_field(long *data)
942 {
943 inv_error_t result = INV_SUCCESS;
944 if (inv_get_state() < INV_STATE_DMP_OPENED)
945 return INV_ERROR_SM_IMPROPER_STATE;
946
947 if (NULL == data) {
948 return INV_ERROR_INVALID_PARAMETER;
949 }
950
951 data[0] = inv_obj.local_field[0];
952 data[1] = inv_obj.local_field[1];
953 data[2] = inv_obj.local_field[2];
954
955 return result;
956 }
957
958 /**
959 * @cond MPL
960 * @brief inv_get_relative_quaternion is used to get relative quaternion.
961 *
962 *
963 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
964 * must have been called.
965 *
966 * @param data
967 * A pointer to an array to be passed back to the user.
968 * <b>Must be 4 cells long at least</b>.
969 *
970 * @return Zero if the command is successful; an ML error code otherwise.
971 * @endcond
972 */
973 /* inv_get_relative_quaternion implemented in mlFIFO.c */
974
975 /**
976 * @brief inv_get_gyro_float is used to get the most recent gyroscope measurement.
977 * The argument array elements are ordered X,Y,Z.
978 * The values are in units of dps (degrees per second).
979 *
980 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
981 * must have been called.
982 *
983 * @param data
984 * A pointer to an array to be passed back to the user.
985 * <b>Must be 3 cells long</b>.
986 *
987 * @return INV_SUCCESS if the command is successful; an error code otherwise.
988 */
inv_get_gyro_float(float * data)989 inv_error_t inv_get_gyro_float(float *data)
990 {
991 INVENSENSE_FUNC_START;
992
993 inv_error_t result = INV_SUCCESS;
994 long ldata[3];
995
996 if (inv_get_state() < INV_STATE_DMP_OPENED)
997 return INV_ERROR_SM_IMPROPER_STATE;
998
999 if (NULL == data) {
1000 return INV_ERROR_INVALID_PARAMETER;
1001 }
1002 result = inv_get_gyro(ldata);
1003 data[0] = (float)ldata[0] / 65536.0f;
1004 data[1] = (float)ldata[1] / 65536.0f;
1005 data[2] = (float)ldata[2] / 65536.0f;
1006
1007 return result;
1008 }
1009
1010 /**
1011 * @internal
1012 * @brief inv_get_angular_velocity_float is used to get an array of three data points representing the angular
1013 * velocity as derived from <b>both</b> gyroscopes and accelerometers.
1014 * This requires that ML_SENSOR_FUSION be enabled, to fuse data from
1015 * the gyroscope and accelerometer device, appropriately scaled and
1016 * oriented according to the respective mounting matrices.
1017 *
1018 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1019 * must have been called.
1020 * @param data
1021 * A pointer to an array to be passed back to the user.
1022 * <b>Must be 3 cells long</b>.
1023 *
1024 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1025 */
inv_get_angular_velocity_float(float * data)1026 inv_error_t inv_get_angular_velocity_float(float *data)
1027 {
1028 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1029 /* not implemented. old (invalid) implementation:
1030 return inv_get_gyro_float(data);
1031 */
1032 }
1033
1034 /**
1035 * @brief inv_get_accel_float is used to get the most recent accelerometer measurement.
1036 * The argument array elements are ordered X,Y,Z.
1037 * The values are in units of g (gravity).
1038 *
1039 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1040 * must have been called.
1041 *
1042 * @param data
1043 * A pointer to an array to be passed back to the user.
1044 * <b>Must be 3 cells long</b>.
1045 *
1046 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1047 */
1048 /* inv_get_accel_float implemented in mlFIFO.c */
1049
1050 /**
1051 * @brief inv_get_temperature_float is used to get the most recent
1052 * temperature measurement.
1053 * The argument array should only have one element.
1054 * The value is in units of deg C (degrees Celsius).
1055 *
1056 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1057 * must have been called.
1058 *
1059 * @param data
1060 * A pointer to data to be passed back to the user.
1061 *
1062 * @return Zero if the command is successful; an ML error code otherwise.
1063 */
inv_get_temperature_float(float * data)1064 inv_error_t inv_get_temperature_float(float *data)
1065 {
1066 INVENSENSE_FUNC_START;
1067
1068 inv_error_t result = INV_SUCCESS;
1069 long ldata[1];
1070
1071 if (inv_get_state() < INV_STATE_DMP_OPENED)
1072 return INV_ERROR_SM_IMPROPER_STATE;
1073
1074 if (NULL == data) {
1075 return INV_ERROR_INVALID_PARAMETER;
1076 }
1077
1078 result = inv_get_temperature(ldata);
1079 data[0] = (float)ldata[0] / 65536.0f;
1080
1081 return result;
1082 }
1083
1084 /**
1085 * @brief inv_get_rot_mat_float is used to get an array of nine data points representing the rotation
1086 * matrix generated from all available sensors.
1087 * The array format will be R11, R12, R13, R21, R22, R23, R31, R32,
1088 * R33, representing the matrix:
1089 * <center>R11 R12 R13</center>
1090 * <center>R21 R22 R23</center>
1091 * <center>R31 R32 R33</center>
1092 * <b>Please refer to the "9-Axis Sensor Fusion Application Note" document,
1093 * section 7 "Sensor Fusion Output", for details regarding rotation
1094 * matrix output</b>.
1095 *
1096 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1097 * must have been called.
1098 *
1099 * @param data
1100 * A pointer to an array to be passed back to the user.
1101 * <b>Must be 9 cells long at least</b>.
1102 *
1103 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1104 */
inv_get_rot_mat_float(float * data)1105 inv_error_t inv_get_rot_mat_float(float *data)
1106 {
1107 INVENSENSE_FUNC_START;
1108
1109 inv_error_t result = INV_SUCCESS;
1110
1111 if (inv_get_state() < INV_STATE_DMP_OPENED)
1112 return INV_ERROR_SM_IMPROPER_STATE;
1113
1114 if (NULL == data) {
1115 return INV_ERROR_INVALID_PARAMETER;
1116 }
1117 {
1118 long qdata[4], rdata[9];
1119 inv_get_quaternion(qdata);
1120 inv_quaternion_to_rotation(qdata, rdata);
1121 data[0] = (float)rdata[0] / 1073741824.0f;
1122 data[1] = (float)rdata[1] / 1073741824.0f;
1123 data[2] = (float)rdata[2] / 1073741824.0f;
1124 data[3] = (float)rdata[3] / 1073741824.0f;
1125 data[4] = (float)rdata[4] / 1073741824.0f;
1126 data[5] = (float)rdata[5] / 1073741824.0f;
1127 data[6] = (float)rdata[6] / 1073741824.0f;
1128 data[7] = (float)rdata[7] / 1073741824.0f;
1129 data[8] = (float)rdata[8] / 1073741824.0f;
1130 }
1131
1132 return result;
1133 }
1134
1135 /**
1136 * @brief inv_get_quaternion_float is used to get the quaternion representation
1137 * of the current sensor fusion solution.
1138 *
1139 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1140 * must have been called.
1141 *
1142 * @param data
1143 * A pointer to an array to be passed back to the user.
1144 * <b>Must be 4 cells long</b>.
1145 *
1146 * @return INV_SUCCESS if the command is successful; an ML error code otherwise.
1147 */
1148 /* inv_get_quaternion_float implemented in mlFIFO.c */
1149
1150 /**
1151 * @brief inv_get_linear_accel_float is used to get an estimate of linear
1152 * acceleration, based on the most recent accelerometer measurement
1153 * and sensor fusion solution.
1154 * The values are in units of g (gravity).
1155 *
1156 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1157 * must have been called.
1158 *
1159 * @param data
1160 * A pointer to an array to be passed back to the user.
1161 * <b>Must be 3 cells long</b>.
1162 *
1163 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1164 */
inv_get_linear_accel_float(float * data)1165 inv_error_t inv_get_linear_accel_float(float *data)
1166 {
1167 INVENSENSE_FUNC_START;
1168
1169 inv_error_t result = INV_SUCCESS;
1170 long ldata[3];
1171
1172 if (inv_get_state() < INV_STATE_DMP_OPENED)
1173 return INV_ERROR_SM_IMPROPER_STATE;
1174
1175 if (NULL == data) {
1176 return INV_ERROR_INVALID_PARAMETER;
1177 }
1178
1179 result = inv_get_linear_accel(ldata);
1180 data[0] = (float)ldata[0] / 65536.0f;
1181 data[1] = (float)ldata[1] / 65536.0f;
1182 data[2] = (float)ldata[2] / 65536.0f;
1183
1184 return result;
1185 }
1186
1187 /**
1188 * @brief inv_get_linear_accel_in_world_float is used to get an estimate of
1189 * linear acceleration, in the world frame, based on the most
1190 * recent accelerometer measurement and sensor fusion solution.
1191 * The values are in units of g (gravity).
1192 *
1193 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1194 * must have been called.
1195 *
1196 * @param data
1197 * A pointer to an array to be passed back to the user.
1198 * <b>Must be 3 cells long</b>.
1199 *
1200 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1201 */
inv_get_linear_accel_in_world_float(float * data)1202 inv_error_t inv_get_linear_accel_in_world_float(float *data)
1203 {
1204 INVENSENSE_FUNC_START;
1205
1206 inv_error_t result = INV_SUCCESS;
1207 long ldata[3];
1208
1209 if (inv_get_state() < INV_STATE_DMP_OPENED)
1210 return INV_ERROR_SM_IMPROPER_STATE;
1211
1212 if (NULL == data) {
1213 return INV_ERROR_INVALID_PARAMETER;
1214 }
1215
1216 result = inv_get_linear_accel_in_world(ldata);
1217 data[0] = (float)ldata[0] / 65536.0f;
1218 data[1] = (float)ldata[1] / 65536.0f;
1219 data[2] = (float)ldata[2] / 65536.0f;
1220
1221 return result;
1222 }
1223
1224 /**
1225 * @brief inv_get_gravity_float is used to get an estimate of the body frame
1226 * gravity vector, based on the most recent sensor fusion solution.
1227 *
1228 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1229 * must have been called.
1230 *
1231 * @param data
1232 * A pointer to an array to be passed back to the user.
1233 * <b>Must be 3 cells long at least</b>.
1234 *
1235 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1236 */
inv_get_gravity_float(float * data)1237 inv_error_t inv_get_gravity_float(float *data)
1238 {
1239 INVENSENSE_FUNC_START;
1240
1241 inv_error_t result = INV_SUCCESS;
1242 long ldata[3];
1243
1244 if (inv_get_state() < INV_STATE_DMP_OPENED)
1245 return INV_ERROR_SM_IMPROPER_STATE;
1246
1247 if (NULL == data) {
1248 return INV_ERROR_INVALID_PARAMETER;
1249 }
1250 result = inv_get_gravity(ldata);
1251 data[0] = (float)ldata[0] / 65536.0f;
1252 data[1] = (float)ldata[1] / 65536.0f;
1253 data[2] = (float)ldata[2] / 65536.0f;
1254
1255 return result;
1256 }
1257
1258 /**
1259 * @brief inv_get_gyro_cal_matrix_float is used to get the gyroscope
1260 * calibration matrix. The gyroscope calibration matrix defines the relationship
1261 * between the gyroscope sensor axes and the sensor fusion solution axes.
1262 * Calibration matrix data members will have a value of 1.0, 0, or -1.0.
1263 * The matrix has members
1264 * <center>C11 C12 C13</center>
1265 * <center>C21 C22 C23</center>
1266 * <center>C31 C32 C33</center>
1267 * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
1268 *
1269 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1270 * must have been called.
1271 *
1272 * @param data
1273 * A pointer to an array to be passed back to the user.
1274 * <b>Must be 9 cells long</b>.
1275 *
1276 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1277 */
inv_get_gyro_cal_matrix_float(float * data)1278 inv_error_t inv_get_gyro_cal_matrix_float(float *data)
1279 {
1280 INVENSENSE_FUNC_START;
1281
1282 inv_error_t result = INV_SUCCESS;
1283
1284 if (inv_get_state() < INV_STATE_DMP_OPENED)
1285 return INV_ERROR_SM_IMPROPER_STATE;
1286
1287 if (NULL == data) {
1288 return INV_ERROR_INVALID_PARAMETER;
1289 }
1290
1291 data[0] = (float)inv_obj.gyro_cal[0] / 1073741824.0f;
1292 data[1] = (float)inv_obj.gyro_cal[1] / 1073741824.0f;
1293 data[2] = (float)inv_obj.gyro_cal[2] / 1073741824.0f;
1294 data[3] = (float)inv_obj.gyro_cal[3] / 1073741824.0f;
1295 data[4] = (float)inv_obj.gyro_cal[4] / 1073741824.0f;
1296 data[5] = (float)inv_obj.gyro_cal[5] / 1073741824.0f;
1297 data[6] = (float)inv_obj.gyro_cal[6] / 1073741824.0f;
1298 data[7] = (float)inv_obj.gyro_cal[7] / 1073741824.0f;
1299 data[8] = (float)inv_obj.gyro_cal[8] / 1073741824.0f;
1300
1301 return result;
1302 }
1303
1304 /**
1305 * @brief inv_get_accel_cal_matrix_float is used to get the accelerometer
1306 * calibration matrix.
1307 * Calibration matrix data members will have a value of 1.0, 0, or -1.0.
1308 * The matrix has members
1309 * <center>C11 C12 C13</center>
1310 * <center>C21 C22 C23</center>
1311 * <center>C31 C32 C33</center>
1312 * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
1313 *
1314 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1315 * must have been called.
1316 *
1317 * @param data
1318 * A pointer to an array to be passed back to the user.
1319 * <b>Must be 9 cells long</b>.
1320 *
1321 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1322 */
1323
inv_get_accel_cal_matrix_float(float * data)1324 inv_error_t inv_get_accel_cal_matrix_float(float *data)
1325 {
1326 INVENSENSE_FUNC_START;
1327
1328 inv_error_t result = INV_SUCCESS;
1329
1330 if (inv_get_state() < INV_STATE_DMP_OPENED)
1331 return INV_ERROR_SM_IMPROPER_STATE;
1332
1333 if (NULL == data) {
1334 return INV_ERROR_INVALID_PARAMETER;
1335 }
1336
1337 data[0] = (float)inv_obj.accel_cal[0] / 1073741824.0f;
1338 data[1] = (float)inv_obj.accel_cal[1] / 1073741824.0f;
1339 data[2] = (float)inv_obj.accel_cal[2] / 1073741824.0f;
1340 data[3] = (float)inv_obj.accel_cal[3] / 1073741824.0f;
1341 data[4] = (float)inv_obj.accel_cal[4] / 1073741824.0f;
1342 data[5] = (float)inv_obj.accel_cal[5] / 1073741824.0f;
1343 data[6] = (float)inv_obj.accel_cal[6] / 1073741824.0f;
1344 data[7] = (float)inv_obj.accel_cal[7] / 1073741824.0f;
1345 data[8] = (float)inv_obj.accel_cal[8] / 1073741824.0f;
1346
1347 return result;
1348 }
1349
1350 /**
1351 * @cond MPL
1352 * @brief inv_get_mag_cal_matrix_float is used to get an array of nine data points
1353 * representing the calibration matrix for the compass:
1354 * <center>C11 C12 C13</center>
1355 * <center>C21 C22 C23</center>
1356 * <center>C31 C32 C33</center>
1357 *
1358 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1359 * must have been called.
1360 *
1361 * @param data
1362 * A pointer to an array to be passed back to the user.
1363 * <b>Must be 9 cells long at least</b>.
1364 *
1365 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1366 * @endcond
1367 */
inv_get_mag_cal_matrix_float(float * data)1368 inv_error_t inv_get_mag_cal_matrix_float(float *data)
1369 {
1370 INVENSENSE_FUNC_START;
1371
1372 inv_error_t result = INV_SUCCESS;
1373
1374 if (inv_get_state() < INV_STATE_DMP_OPENED)
1375 return INV_ERROR_SM_IMPROPER_STATE;
1376
1377 if (NULL == data) {
1378 return INV_ERROR_INVALID_PARAMETER;
1379 }
1380
1381 data[0] = (float)inv_obj.compass_cal[0] / 1073741824.0f;
1382 data[1] = (float)inv_obj.compass_cal[1] / 1073741824.0f;
1383 data[2] = (float)inv_obj.compass_cal[2] / 1073741824.0f;
1384 data[3] = (float)inv_obj.compass_cal[3] / 1073741824.0f;
1385 data[4] = (float)inv_obj.compass_cal[4] / 1073741824.0f;
1386 data[5] = (float)inv_obj.compass_cal[5] / 1073741824.0f;
1387 data[6] = (float)inv_obj.compass_cal[6] / 1073741824.0f;
1388 data[7] = (float)inv_obj.compass_cal[7] / 1073741824.0f;
1389 data[8] = (float)inv_obj.compass_cal[8] / 1073741824.0f;
1390 return result;
1391 }
1392
1393 /**
1394 * @brief inv_get_gyro_temp_slope_float is used to get the temperature
1395 * compensation algorithm's estimate of the gyroscope bias temperature
1396 * coefficient.
1397 * The argument array elements are ordered X,Y,Z.
1398 * Values are in units of dps per deg C (degrees per second per degree
1399 * Celcius)
1400 * Please refer to the provided "9-Axis Sensor Fusion
1401 * Application Note" document.
1402 *
1403 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1404 * must have been called.
1405 *
1406 * @param data
1407 * A pointer to an array to be passed back to the user.
1408 * <b>Must be 3 cells long </b>.
1409 *
1410 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1411 */
inv_get_gyro_temp_slope_float(float * data)1412 inv_error_t inv_get_gyro_temp_slope_float(float *data)
1413 {
1414 INVENSENSE_FUNC_START;
1415
1416 inv_error_t result = INV_SUCCESS;
1417
1418 if (inv_get_state() < INV_STATE_DMP_OPENED)
1419 return INV_ERROR_SM_IMPROPER_STATE;
1420
1421 if (NULL == data) {
1422 return INV_ERROR_INVALID_PARAMETER;
1423 }
1424
1425 if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
1426 data[0] = inv_obj.x_gyro_coef[1];
1427 data[1] = inv_obj.y_gyro_coef[1];
1428 data[2] = inv_obj.z_gyro_coef[1];
1429 } else {
1430 data[0] = (float)inv_obj.temp_slope[0] / 65536.0f;
1431 data[1] = (float)inv_obj.temp_slope[1] / 65536.0f;
1432 data[2] = (float)inv_obj.temp_slope[2] / 65536.0f;
1433 }
1434
1435 return result;
1436 }
1437
1438 /**
1439 * @brief inv_get_gyro_bias_float is used to get the gyroscope biases.
1440 * The argument array elements are ordered X,Y,Z.
1441 * The values are in units of dps (degrees per second).
1442
1443 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1444 * must have been called.
1445 *
1446 * @param data
1447 * A pointer to an array to be passed back to the user.
1448 * <b>Must be 3 cells long</b>.
1449 *
1450 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1451 */
inv_get_gyro_bias_float(float * data)1452 inv_error_t inv_get_gyro_bias_float(float *data)
1453 {
1454 INVENSENSE_FUNC_START;
1455
1456 inv_error_t result = INV_SUCCESS;
1457
1458 if (inv_get_state() < INV_STATE_DMP_OPENED)
1459 return INV_ERROR_SM_IMPROPER_STATE;
1460
1461 if (NULL == data) {
1462 return INV_ERROR_INVALID_PARAMETER;
1463 }
1464
1465 data[0] = (float)inv_obj.gyro_bias[0] / 65536.0f;
1466 data[1] = (float)inv_obj.gyro_bias[1] / 65536.0f;
1467 data[2] = (float)inv_obj.gyro_bias[2] / 65536.0f;
1468
1469 return result;
1470 }
1471
1472 /**
1473 * @brief inv_get_accel_bias_float is used to get the accelerometer baises.
1474 * The argument array elements are ordered X,Y,Z.
1475 * The values are in units of g (gravity).
1476 *
1477 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1478 * must have been called.
1479 *
1480 * @param data
1481 * A pointer to an array to be passed back to the user.
1482 * <b>Must be 3 cells long</b>.
1483 *
1484 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1485 */
inv_get_accel_bias_float(float * data)1486 inv_error_t inv_get_accel_bias_float(float *data)
1487 {
1488 INVENSENSE_FUNC_START;
1489
1490 inv_error_t result = INV_SUCCESS;
1491
1492 if (inv_get_state() < INV_STATE_DMP_OPENED)
1493 return INV_ERROR_SM_IMPROPER_STATE;
1494
1495 if (NULL == data) {
1496 return INV_ERROR_INVALID_PARAMETER;
1497 }
1498
1499 data[0] = (float)inv_obj.accel_bias[0] / 65536.0f;
1500 data[1] = (float)inv_obj.accel_bias[1] / 65536.0f;
1501 data[2] = (float)inv_obj.accel_bias[2] / 65536.0f;
1502
1503 return result;
1504 }
1505
1506 /**
1507 * @cond MPL
1508 * @brief inv_get_mag_bias_float is used to get an array of three data points representing
1509 * the compass biases.
1510 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1511 * must have been called.
1512 *
1513 * @param data
1514 * A pointer to an array to be passed back to the user.
1515 * <b>Must be 3 cells long</b>.
1516 *
1517 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1518 * @endcond
1519 */
inv_get_mag_bias_float(float * data)1520 inv_error_t inv_get_mag_bias_float(float *data)
1521 {
1522 INVENSENSE_FUNC_START;
1523
1524 inv_error_t result = INV_SUCCESS;
1525
1526 if (inv_get_state() < INV_STATE_DMP_OPENED)
1527 return INV_ERROR_SM_IMPROPER_STATE;
1528
1529 if (NULL == data) {
1530 return INV_ERROR_INVALID_PARAMETER;
1531 }
1532
1533 data[0] =
1534 ((float)
1535 (inv_obj.compass_bias[0] +
1536 (long)((long long)inv_obj.init_compass_bias[0] *
1537 inv_obj.compass_sens / 16384))) / 65536.0f;
1538 data[1] =
1539 ((float)
1540 (inv_obj.compass_bias[1] +
1541 (long)((long long)inv_obj.init_compass_bias[1] *
1542 inv_obj.compass_sens / 16384))) / 65536.0f;
1543 data[2] =
1544 ((float)
1545 (inv_obj.compass_bias[2] +
1546 (long)((long long)inv_obj.init_compass_bias[2] *
1547 inv_obj.compass_sens / 16384))) / 65536.0f;
1548
1549 return result;
1550 }
1551
1552 /**
1553 * @brief inv_get_gyro_and_accel_sensor_float is used to get the most recent set of all sensor data.
1554 * The argument array elements are ordered gyroscope X,Y, and Z,
1555 * accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
1556 * \if UMPL The magnetometer elements are not populated in UMPL. \endif
1557 * The gyroscope and accelerometer data is not scaled or offset, it is
1558 * copied directly from the sensor registers, and cast as a float.
1559 * In the case of accelerometers with 8-bit output resolution, the data
1560 * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
1561 * full scale range
1562
1563 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1564 * must have been called.
1565 *
1566 * @param data
1567 * A pointer to an array to be passed back to the user.
1568 * <b>Must be 9 cells long</b>.
1569 *
1570 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1571 */
inv_get_gyro_and_accel_sensor_float(float * data)1572 inv_error_t inv_get_gyro_and_accel_sensor_float(float *data)
1573 {
1574 INVENSENSE_FUNC_START;
1575
1576 inv_error_t result = INV_SUCCESS;
1577 long ldata[6];
1578
1579 if (inv_get_state() < INV_STATE_DMP_OPENED)
1580 return INV_ERROR_SM_IMPROPER_STATE;
1581
1582 if (NULL == data) {
1583 return INV_ERROR_INVALID_PARAMETER;
1584 }
1585
1586 result = inv_get_gyro_and_accel_sensor(ldata);
1587 data[0] = (float)ldata[0];
1588 data[1] = (float)ldata[1];
1589 data[2] = (float)ldata[2];
1590 data[3] = (float)ldata[3];
1591 data[4] = (float)ldata[4];
1592 data[5] = (float)ldata[5];
1593 data[6] = (float)inv_obj.compass_sensor_data[0];
1594 data[7] = (float)inv_obj.compass_sensor_data[1];
1595 data[8] = (float)inv_obj.compass_sensor_data[2];
1596
1597 return result;
1598 }
1599
1600 /**
1601 * @brief inv_get_euler_angles_x is used to get the Euler angle representation
1602 * of the current sensor fusion solution.
1603 * Euler angles are returned according to the X convention.
1604 * This is typically the convention used for mobile devices where the X
1605 * axis is the width of the screen, Y axis is the height, and Z the
1606 * depth. In this case roll is defined as the rotation around the X
1607 * axis of the device.
1608 * <TABLE>
1609 * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
1610 * <TR><TD> 0 </TD><TD>Roll </TD><TD>X axis </TD></TR>
1611 * <TR><TD> 1 </TD><TD>Pitch </TD><TD>Y axis </TD></TR>
1612 * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
1613 *
1614 </TABLE>
1615 *
1616 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1617 * must have been called.
1618 *
1619 * @param data
1620 * A pointer to an array to be passed back to the user.
1621 * <b>Must be 3 cells long</b>.
1622 *
1623 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1624 */
inv_get_euler_angles_x_float(float * data)1625 inv_error_t inv_get_euler_angles_x_float(float *data)
1626 {
1627 INVENSENSE_FUNC_START;
1628
1629 inv_error_t result = INV_SUCCESS;
1630 float rotMatrix[9];
1631 float tmp;
1632
1633 if (inv_get_state() < INV_STATE_DMP_OPENED)
1634 return INV_ERROR_SM_IMPROPER_STATE;
1635
1636 if (NULL == data) {
1637 return INV_ERROR_INVALID_PARAMETER;
1638 }
1639
1640 result = inv_get_rot_mat_float(rotMatrix);
1641 tmp = rotMatrix[6];
1642 if (tmp > 1.0f) {
1643 tmp = 1.0f;
1644 }
1645 if (tmp < -1.0f) {
1646 tmp = -1.0f;
1647 }
1648 data[0] =
1649 (float)(atan2f(rotMatrix[7],
1650 rotMatrix[8]) * 57.29577951308);
1651 data[1] = (float)((double)asin(tmp) * 57.29577951308);
1652 data[2] =
1653 (float)(atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308);
1654
1655 return result;
1656 }
1657
1658 /**
1659 * @brief inv_get_euler_angles_float is used to get an array of three data points three data points
1660 * representing roll, pitch, and yaw corresponding to the INV_EULER_ANGLES_X output and it is
1661 * therefore the default convention for Euler angles.
1662 * Please refer to the INV_EULER_ANGLES_X for a detailed description.
1663 *
1664 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1665 * must have been called.
1666 *
1667 * @param data
1668 * A pointer to an array to be passed back to the user.
1669 * <b>Must be 3 cells long</b>.
1670 *
1671 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1672 */
inv_get_euler_angles_float(float * data)1673 inv_error_t inv_get_euler_angles_float(float *data)
1674 {
1675 return inv_get_euler_angles_x_float(data);
1676 }
1677
1678 /** @brief inv_get_euler_angles_y_float is used to get the Euler angle representation
1679 * of the current sensor fusion solution.
1680 * Euler angles are returned according to the Y convention.
1681 * This convention is typically used in augmented reality applications,
1682 * where roll is defined as the rotation around the axis along the
1683 * height of the screen of a mobile device, namely the Y axis.
1684 * <TABLE>
1685 * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
1686 * <TR><TD> 0 </TD><TD>Roll </TD><TD>Y axis </TD></TR>
1687 * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
1688 * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
1689 * </TABLE>
1690 *
1691 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1692 * must have been called.
1693 *
1694 * @param data
1695 * A pointer to an array to be passed back to the user.
1696 * <b>Must be 3 cells long</b>.
1697 *
1698 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1699 */
inv_get_euler_angles_y_float(float * data)1700 inv_error_t inv_get_euler_angles_y_float(float *data)
1701 {
1702 INVENSENSE_FUNC_START;
1703
1704 inv_error_t result = INV_SUCCESS;
1705 float rotMatrix[9];
1706 float tmp;
1707
1708 if (inv_get_state() < INV_STATE_DMP_OPENED)
1709 return INV_ERROR_SM_IMPROPER_STATE;
1710
1711 if (NULL == data) {
1712 return INV_ERROR_INVALID_PARAMETER;
1713 }
1714
1715 result = inv_get_rot_mat_float(rotMatrix);
1716 tmp = rotMatrix[7];
1717 if (tmp > 1.0f) {
1718 tmp = 1.0f;
1719 }
1720 if (tmp < -1.0f) {
1721 tmp = -1.0f;
1722 }
1723 data[0] =
1724 (float)(atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308);
1725 data[1] = (float)((double)asin(tmp) * 57.29577951308);
1726 data[2] =
1727 (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308);
1728
1729 return result;
1730 }
1731
1732 /** @brief inv_get_euler_angles_z_float is used to get the Euler angle representation
1733 * of the current sensor fusion solution.
1734 * This convention is mostly used in application involving the use
1735 * of a camera, typically placed on the back of a mobile device, that
1736 * is along the Z axis. In this convention roll is defined as the
1737 * rotation around the Z axis.
1738 * Euler angles are returned according to the Y convention.
1739 * <TABLE>
1740 * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
1741 * <TR><TD> 0 </TD><TD>Roll </TD><TD>Z axis </TD></TR>
1742 * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
1743 * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Y axis </TD></TR>
1744 * </TABLE>
1745 *
1746 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1747 * must have been called.
1748 *
1749 * @param data
1750 * A pointer to an array to be passed back to the user.
1751 * <b>Must be 3 cells long</b>.
1752 *
1753 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1754 * must have been called.
1755 *
1756 * @param data
1757 * A pointer to an array to be passed back to the user.
1758 * <b>Must be 3 cells long</b>.
1759 *
1760 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1761 */
inv_get_euler_angles_z_float(float * data)1762 inv_error_t inv_get_euler_angles_z_float(float *data)
1763 {
1764 INVENSENSE_FUNC_START;
1765
1766 inv_error_t result = INV_SUCCESS;
1767 float rotMatrix[9];
1768 float tmp;
1769
1770 if (inv_get_state() < INV_STATE_DMP_OPENED)
1771 return INV_ERROR_SM_IMPROPER_STATE;
1772
1773 if (NULL == data) {
1774 return INV_ERROR_INVALID_PARAMETER;
1775 }
1776
1777 result = inv_get_rot_mat_float(rotMatrix);
1778 tmp = rotMatrix[8];
1779 if (tmp > 1.0f) {
1780 tmp = 1.0f;
1781 }
1782 if (tmp < -1.0f) {
1783 tmp = -1.0f;
1784 }
1785 data[0] =
1786 (float)(atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308);
1787 data[1] = (float)((double)asin(tmp) * 57.29577951308);
1788 data[2] =
1789 (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308);
1790
1791 return result;
1792 }
1793
1794 /**
1795 * @cond MPL
1796 * @brief inv_get_mag_raw_data_float is used to get Raw magnetometer data
1797 *
1798 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1799 * must have been called.
1800 *
1801 * @param data
1802 * A pointer to an array to be passed back to the user.
1803 * <b>Must be 3 cells long</b>.
1804 *
1805 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1806 * @endcond
1807 */
inv_get_mag_raw_data_float(float * data)1808 inv_error_t inv_get_mag_raw_data_float(float *data)
1809 {
1810 INVENSENSE_FUNC_START;
1811
1812 inv_error_t result = INV_SUCCESS;
1813
1814 if (inv_get_state() < INV_STATE_DMP_OPENED)
1815 return INV_ERROR_SM_IMPROPER_STATE;
1816
1817 if (NULL == data) {
1818 return INV_ERROR_INVALID_PARAMETER;
1819 }
1820
1821 data[0] =
1822 (float)(inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]);
1823 data[1] =
1824 (float)(inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]);
1825 data[2] =
1826 (float)(inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]);
1827
1828 return result;
1829 }
1830
1831 /**
1832 * @cond MPL
1833 * @brief inv_get_magnetometer_float is used to get magnetometer data
1834 *
1835 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1836 * must have been called.
1837 *
1838 * @param data
1839 * A pointer to an array to be passed back to the user.
1840 * <b>Must be 3 cells long</b>.
1841 *
1842 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1843 * @endcond
1844 */
inv_get_magnetometer_float(float * data)1845 inv_error_t inv_get_magnetometer_float(float *data)
1846 {
1847 INVENSENSE_FUNC_START;
1848
1849 inv_error_t result = INV_SUCCESS;
1850
1851 if (inv_get_state() < INV_STATE_DMP_OPENED)
1852 return INV_ERROR_SM_IMPROPER_STATE;
1853
1854 if (NULL == data) {
1855 return INV_ERROR_INVALID_PARAMETER;
1856 }
1857
1858 data[0] = (float)inv_obj.compass_calibrated_data[0] / 65536.0f;
1859 data[1] = (float)inv_obj.compass_calibrated_data[1] / 65536.0f;
1860 data[2] = (float)inv_obj.compass_calibrated_data[2] / 65536.0f;
1861
1862 return result;
1863 }
1864
1865 /**
1866 * @cond MPL
1867 * @brief inv_get_pressure_float is used to get a single value representing the pressure in Pascal
1868 *
1869 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1870 * must have been called.
1871 *
1872 * @param data
1873 * A pointer to the data to be passed back to the user.
1874 *
1875 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1876 * @endcond
1877 */
inv_get_pressure_float(float * data)1878 inv_error_t inv_get_pressure_float(float *data)
1879 {
1880 INVENSENSE_FUNC_START;
1881
1882 inv_error_t result = INV_SUCCESS;
1883
1884 if (inv_get_state() < INV_STATE_DMP_OPENED)
1885 return INV_ERROR_SM_IMPROPER_STATE;
1886
1887 if (NULL == data) {
1888 return INV_ERROR_INVALID_PARAMETER;
1889 }
1890
1891 data[0] = (float)inv_obj.pressure;
1892
1893 return result;
1894 }
1895
1896 /**
1897 * @cond MPL
1898 * @brief inv_get_heading_float is used to get single number representing the heading of the device
1899 * relative to the Earth, in which 0 represents North, 90 degrees
1900 * represents East, and so on.
1901 * The heading is defined as the direction of the +Y axis if the Y
1902 * axis is horizontal, and otherwise the direction of the -Z axis.
1903 *
1904 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1905 * must have been called.
1906 *
1907 * @param data
1908 * A pointer to the data to be passed back to the user.
1909 *
1910 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1911 * @endcond
1912 */
inv_get_heading_float(float * data)1913 inv_error_t inv_get_heading_float(float *data)
1914 {
1915 INVENSENSE_FUNC_START;
1916
1917 inv_error_t result = INV_SUCCESS;
1918 float rotMatrix[9];
1919 float tmp;
1920
1921 if (inv_get_state() < INV_STATE_DMP_OPENED)
1922 return INV_ERROR_SM_IMPROPER_STATE;
1923
1924 if (NULL == data) {
1925 return INV_ERROR_INVALID_PARAMETER;
1926 }
1927
1928 inv_get_rot_mat_float(rotMatrix);
1929 if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
1930 tmp =
1931 (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
1932 90.0f);
1933 } else {
1934 tmp =
1935 (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
1936 90.0f);
1937 }
1938 if (tmp < 0) {
1939 tmp += 360.0f;
1940 }
1941 data[0] = 360 - tmp;
1942
1943 return result;
1944 }
1945
1946 /**
1947 * @cond MPL
1948 * @brief inv_get_mag_bias_error_float is used to get an array of three numbers representing
1949 * the current estimated error in the compass biases. These numbers are unitless and serve
1950 * as rough estimates in which numbers less than 100 typically represent
1951 * reasonably well calibrated compass axes.
1952 *
1953 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1954 * must have been called.
1955 *
1956 * @param data
1957 * A pointer to an array to be passed back to the user.
1958 * <b>Must be 3 cells long</b>.
1959 *
1960 * @return INV_SUCCESS if the command is successful; an error code otherwise.
1961 * @endcond
1962 */
inv_get_mag_bias_error_float(float * data)1963 inv_error_t inv_get_mag_bias_error_float(float *data)
1964 {
1965 INVENSENSE_FUNC_START;
1966
1967 inv_error_t result = INV_SUCCESS;
1968
1969 if (inv_get_state() < INV_STATE_DMP_OPENED)
1970 return INV_ERROR_SM_IMPROPER_STATE;
1971
1972 if (NULL == data) {
1973 return INV_ERROR_INVALID_PARAMETER;
1974 }
1975
1976 if (inv_obj.large_field == 0) {
1977 data[0] = (float)inv_obj.compass_bias_error[0];
1978 data[1] = (float)inv_obj.compass_bias_error[1];
1979 data[2] = (float)inv_obj.compass_bias_error[2];
1980 } else {
1981 data[0] = (float)P_INIT;
1982 data[1] = (float)P_INIT;
1983 data[2] = (float)P_INIT;
1984 }
1985
1986 return result;
1987 }
1988
1989 /**
1990 * @cond MPL
1991 * @brief inv_get_mag_scale_float is used to get magnetometer scale.
1992 *
1993 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
1994 * must have been called.
1995 *
1996 * @param data
1997 * A pointer to an array to be passed back to the user.
1998 * <b>Must be 3 cells long</b>.
1999 *
2000 * @return INV_SUCCESS if the command is successful; an error code otherwise.
2001 * @endcond
2002 */
inv_get_mag_scale_float(float * data)2003 inv_error_t inv_get_mag_scale_float(float *data)
2004 {
2005 INVENSENSE_FUNC_START;
2006
2007 inv_error_t result = INV_SUCCESS;
2008
2009 if (inv_get_state() < INV_STATE_DMP_OPENED)
2010 return INV_ERROR_SM_IMPROPER_STATE;
2011
2012 if (NULL == data) {
2013 return INV_ERROR_INVALID_PARAMETER;
2014 }
2015
2016 data[0] = (float)inv_obj.compass_scale[0] / 65536.0f;
2017 data[1] = (float)inv_obj.compass_scale[1] / 65536.0f;
2018 data[2] = (float)inv_obj.compass_scale[2] / 65536.0f;
2019
2020 return result;
2021 }
2022
2023 /**
2024 * @cond MPL
2025 * @brief inv_get_local_field_float is used to get local magnetic field data.
2026 *
2027 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
2028 * must have been called.
2029 *
2030 * @param data
2031 * A pointer to an array to be passed back to the user.
2032 * <b>Must be 3 cells long</b>.
2033 *
2034 * @return INV_SUCCESS if the command is successful; an error code otherwise.
2035 * @endcond
2036 */
inv_get_local_field_float(float * data)2037 inv_error_t inv_get_local_field_float(float *data)
2038 {
2039 INVENSENSE_FUNC_START;
2040
2041 inv_error_t result = INV_SUCCESS;
2042
2043 if (inv_get_state() < INV_STATE_DMP_OPENED)
2044 return INV_ERROR_SM_IMPROPER_STATE;
2045
2046 if (NULL == data) {
2047 return INV_ERROR_INVALID_PARAMETER;
2048 }
2049
2050 data[0] = (float)inv_obj.local_field[0] / 65536.0f;
2051 data[1] = (float)inv_obj.local_field[1] / 65536.0f;
2052 data[2] = (float)inv_obj.local_field[2] / 65536.0f;
2053
2054 return result;
2055 }
2056
2057 /**
2058 * @cond MPL
2059 * @brief inv_get_relative_quaternion_float is used to get relative quaternion data.
2060 *
2061 * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
2062 * must have been called.
2063 *
2064 * @param data
2065 * A pointer to an array to be passed back to the user.
2066 * <b>Must be 4 cells long at least</b>.
2067 *
2068 * @return INV_SUCCESS if the command is successful; an error code otherwise.
2069 * @endcond
2070 */
inv_get_relative_quaternion_float(float * data)2071 inv_error_t inv_get_relative_quaternion_float(float *data)
2072 {
2073 INVENSENSE_FUNC_START;
2074
2075 inv_error_t result = INV_SUCCESS;
2076
2077 if (inv_get_state() < INV_STATE_DMP_OPENED)
2078 return INV_ERROR_SM_IMPROPER_STATE;
2079
2080 if (NULL == data) {
2081 return INV_ERROR_INVALID_PARAMETER;
2082 }
2083
2084 data[0] = (float)inv_obj.relative_quat[0] / 1073741824.0f;
2085 data[1] = (float)inv_obj.relative_quat[1] / 1073741824.0f;
2086 data[2] = (float)inv_obj.relative_quat[2] / 1073741824.0f;
2087 data[3] = (float)inv_obj.relative_quat[3] / 1073741824.0f;
2088
2089 return result;
2090 }
2091
2092 /**
2093 * Returns the curren compass accuracy.
2094 *
2095 * - 0: Unknown: The accuracy is unreliable and compass data should not be used
2096 * - 1: Low: The compass accuracy is low.
2097 * - 2: Medium: The compass accuracy is medium.
2098 * - 3: High: The compas acurracy is high and can be trusted
2099 *
2100 * @param accuracy The accuracy level in the range 0-3
2101 *
2102 * @return ML_SUCCESS or non-zero error code
2103 */
inv_get_compass_accuracy(int * accuracy)2104 inv_error_t inv_get_compass_accuracy(int *accuracy)
2105 {
2106 if (inv_get_state() != INV_STATE_DMP_STARTED)
2107 return INV_ERROR_SM_IMPROPER_STATE;
2108
2109 *accuracy = inv_obj.compass_accuracy;
2110 return INV_SUCCESS;
2111 }
2112
2113 /**
2114 * @brief inv_set_gyro_bias is used to set the gyroscope bias.
2115 * The argument array elements are ordered X,Y,Z.
2116 * The values are scaled at 1 dps = 2^16 LSBs.
2117 *
2118 * Please refer to the provided "9-Axis Sensor Fusion
2119 * Application Note" document provided.
2120 *
2121 * @pre MLDmpOpen() \ifnot UMPL or
2122 * MLDmpPedometerStandAloneOpen() \endif
2123 *
2124 * @param data A pointer to an array to be copied from the user.
2125 *
2126 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2127 */
inv_set_gyro_bias(long * data)2128 inv_error_t inv_set_gyro_bias(long *data)
2129 {
2130 INVENSENSE_FUNC_START;
2131 inv_error_t result = INV_SUCCESS;
2132 long biasTmp;
2133 long sf = 0;
2134 short offset[GYRO_NUM_AXES];
2135 int i;
2136 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
2137
2138 if (mldl_cfg->gyro_sens_trim != 0) {
2139 sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
2140 } else {
2141 sf = 2000;
2142 }
2143 for (i = 0; i < GYRO_NUM_AXES; i++) {
2144 inv_obj.gyro_bias[i] = data[i];
2145 biasTmp = -inv_obj.gyro_bias[i] / sf;
2146 if (biasTmp < 0)
2147 biasTmp += 65536L;
2148 offset[i] = (short)biasTmp;
2149 }
2150 result = inv_set_offset(offset);
2151 if (result) {
2152 LOG_RESULT_LOCATION(result);
2153 return result;
2154 }
2155
2156 return INV_SUCCESS;
2157 }
2158
2159 /**
2160 * @brief inv_set_accel_bias is used to set the accelerometer bias.
2161 * The argument array elements are ordered X,Y,Z.
2162 * The values are scaled in units of g (gravity),
2163 * where 1 g = 2^16 LSBs.
2164 *
2165 * Please refer to the provided "9-Axis Sensor Fusion
2166 * Application Note" document provided.
2167 *
2168 * @pre MLDmpOpen() \ifnot UMPL or
2169 * MLDmpPedometerStandAloneOpen() \endif
2170 *
2171 * @param data A pointer to an array to be copied from the user.
2172 *
2173 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2174 */
inv_set_accel_bias(long * data)2175 inv_error_t inv_set_accel_bias(long *data)
2176 {
2177 INVENSENSE_FUNC_START;
2178 inv_error_t result = INV_SUCCESS;
2179 long biasTmp;
2180 int i, j;
2181 unsigned char regs[6];
2182 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
2183
2184 for (i = 0; i < ACCEL_NUM_AXES; i++) {
2185 inv_obj.accel_bias[i] = data[i];
2186 if (inv_obj.accel_sens != 0 && mldl_cfg && mldl_cfg->pdata) {
2187 long long tmp64;
2188 inv_obj.scaled_accel_bias[i] = 0;
2189 for (j = 0; j < ACCEL_NUM_AXES; j++) {
2190 inv_obj.scaled_accel_bias[i] +=
2191 data[j] *
2192 (long)mldl_cfg->pdata->accel.orientation[i * 3 + j];
2193 }
2194 tmp64 = (long long)inv_obj.scaled_accel_bias[i] << 13;
2195 biasTmp = (long)(tmp64 / inv_obj.accel_sens);
2196 } else {
2197 biasTmp = 0;
2198 }
2199 if (biasTmp < 0)
2200 biasTmp += 65536L;
2201 regs[2 * i + 0] = (unsigned char)(biasTmp / 256);
2202 regs[2 * i + 1] = (unsigned char)(biasTmp % 256);
2203 }
2204 result = inv_set_mpu_memory(KEY_D_1_8, 2, ®s[0]);
2205 if (result) {
2206 LOG_RESULT_LOCATION(result);
2207 return result;
2208 }
2209 result = inv_set_mpu_memory(KEY_D_1_10, 2, ®s[2]);
2210 if (result) {
2211 LOG_RESULT_LOCATION(result);
2212 return result;
2213 }
2214 result = inv_set_mpu_memory(KEY_D_1_2, 2, ®s[4]);
2215 if (result) {
2216 LOG_RESULT_LOCATION(result);
2217 return result;
2218 }
2219
2220 return INV_SUCCESS;
2221 }
2222
2223 /**
2224 * @cond MPL
2225 * @brief inv_set_mag_bias is used to set Compass Bias
2226 *
2227 * Please refer to the provided "9-Axis Sensor Fusion
2228 * Application Note" document provided.
2229 *
2230 * @pre MLDmpOpen() \ifnot UMPL or
2231 * MLDmpPedometerStandAloneOpen() \endif
2232 * @pre MLDmpStart() must <b>NOT</b> have been called.
2233 *
2234 * @param data A pointer to an array to be copied from the user.
2235 *
2236 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2237 * @endcond
2238 */
inv_set_mag_bias(long * data)2239 inv_error_t inv_set_mag_bias(long *data)
2240 {
2241 INVENSENSE_FUNC_START;
2242 inv_error_t result = INV_SUCCESS;
2243
2244 inv_set_compass_bias(data);
2245 inv_obj.init_compass_bias[0] = 0;
2246 inv_obj.init_compass_bias[1] = 0;
2247 inv_obj.init_compass_bias[2] = 0;
2248 inv_obj.got_compass_bias = 1;
2249 inv_obj.got_init_compass_bias = 1;
2250 inv_obj.compass_state = SF_STARTUP_SETTLE;
2251
2252 if (result) {
2253 LOG_RESULT_LOCATION(result);
2254 return result;
2255 }
2256 return INV_SUCCESS;
2257 }
2258
2259 /**
2260 * @brief inv_set_gyro_temp_slope is used to set the temperature
2261 * compensation algorithm's estimate of the gyroscope bias temperature
2262 * coefficient.
2263 * The argument array elements are ordered X,Y,Z.
2264 * Values are in units of dps per deg C (degrees per second per degree
2265 * Celcius), and scaled such that 1 dps per deg C = 2^16 LSBs.
2266 * Please refer to the provided "9-Axis Sensor Fusion
2267 * Application Note" document.
2268 *
2269 * @brief inv_set_gyro_temp_slope is used to set Gyro temperature slope
2270 *
2271 *
2272 * @pre MLDmpOpen() \ifnot UMPL or
2273 * MLDmpPedometerStandAloneOpen() \endif
2274 *
2275 * @param data A pointer to an array to be copied from the user.
2276 *
2277 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2278 */
inv_set_gyro_temp_slope(long * data)2279 inv_error_t inv_set_gyro_temp_slope(long *data)
2280 {
2281 INVENSENSE_FUNC_START;
2282 inv_error_t result = INV_SUCCESS;
2283 int i;
2284 long sf;
2285 unsigned char regs[3];
2286
2287 inv_obj.factory_temp_comp = 1;
2288 inv_obj.temp_slope[0] = data[0];
2289 inv_obj.temp_slope[1] = data[1];
2290 inv_obj.temp_slope[2] = data[2];
2291 for (i = 0; i < GYRO_NUM_AXES; i++) {
2292 sf = -inv_obj.temp_slope[i] / 1118;
2293 if (sf > 127) {
2294 sf -= 256;
2295 }
2296 regs[i] = (unsigned char)sf;
2297 }
2298 result = inv_set_offsetTC(regs);
2299
2300 if (result) {
2301 LOG_RESULT_LOCATION(result);
2302 return result;
2303 }
2304 return INV_SUCCESS;
2305 }
2306
2307 /**
2308 * @cond MPL
2309 * @brief inv_set_local_field is used to set local magnetic field
2310 *
2311 * Please refer to the provided "9-Axis Sensor Fusion
2312 * Application Note" document provided.
2313 *
2314 * @pre MLDmpOpen() \ifnot UMPL or
2315 * MLDmpPedometerStandAloneOpen() \endif
2316 * @pre MLDmpStart() must <b>NOT</b> have been called.
2317 *
2318 * @param data A pointer to an array to be copied from the user.
2319 *
2320 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2321 * @endcond
2322 */
inv_set_local_field(long * data)2323 inv_error_t inv_set_local_field(long *data)
2324 {
2325 INVENSENSE_FUNC_START;
2326 inv_error_t result = INV_SUCCESS;
2327
2328 inv_obj.local_field[0] = data[0];
2329 inv_obj.local_field[1] = data[1];
2330 inv_obj.local_field[2] = data[2];
2331 inv_obj.new_local_field = 1;
2332
2333 if (result) {
2334 LOG_RESULT_LOCATION(result);
2335 return result;
2336 }
2337 return INV_SUCCESS;
2338 }
2339
2340 /**
2341 * @cond MPL
2342 * @brief inv_set_mag_scale is used to set magnetometer scale
2343 *
2344 * Please refer to the provided "9-Axis Sensor Fusion
2345 * Application Note" document provided.
2346 *
2347 * @pre MLDmpOpen() \ifnot UMPL or
2348 * MLDmpPedometerStandAloneOpen() \endif
2349 * @pre MLDmpStart() must <b>NOT</b> have been called.
2350 *
2351 * @param data A pointer to an array to be copied from the user.
2352 *
2353 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2354 * @endcond
2355 */
inv_set_mag_scale(long * data)2356 inv_error_t inv_set_mag_scale(long *data)
2357 {
2358 INVENSENSE_FUNC_START;
2359 inv_error_t result = INV_SUCCESS;
2360
2361 inv_obj.compass_scale[0] = data[0];
2362 inv_obj.compass_scale[1] = data[1];
2363 inv_obj.compass_scale[2] = data[2];
2364
2365 if (result) {
2366 LOG_RESULT_LOCATION(result);
2367 return result;
2368 }
2369 return INV_SUCCESS;
2370 }
2371
2372 /**
2373 * @brief inv_set_gyro_temp_slope_float is used to get the temperature
2374 * compensation algorithm's estimate of the gyroscope bias temperature
2375 * coefficient.
2376 * The argument array elements are ordered X,Y,Z.
2377 * Values are in units of dps per deg C (degrees per second per degree
2378 * Celcius)
2379
2380 * Please refer to the provided "9-Axis Sensor Fusion
2381 * Application Note" document provided.
2382 *
2383 * @pre MLDmpOpen() \ifnot UMPL or
2384 * MLDmpPedometerStandAloneOpen() \endif
2385 *
2386 * @param data A pointer to an array to be copied from the user.
2387 *
2388 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2389 */
inv_set_gyro_temp_slope_float(float * data)2390 inv_error_t inv_set_gyro_temp_slope_float(float *data)
2391 {
2392 long arrayTmp[3];
2393 arrayTmp[0] = (long)(data[0] * 65536.f);
2394 arrayTmp[1] = (long)(data[1] * 65536.f);
2395 arrayTmp[2] = (long)(data[2] * 65536.f);
2396 return inv_set_gyro_temp_slope(arrayTmp);
2397 }
2398
2399 /**
2400 * @brief inv_set_gyro_bias_float is used to set the gyroscope bias.
2401 * The argument array elements are ordered X,Y,Z.
2402 * The values are in units of dps (degrees per second).
2403 *
2404 * Please refer to the provided "9-Axis Sensor Fusion
2405 * Application Note" document provided.
2406 *
2407 * @pre MLDmpOpen() \ifnot UMPL or
2408 * MLDmpPedometerStandAloneOpen() \endif
2409 * @pre MLDmpStart() must <b>NOT</b> have been called.
2410 *
2411 * @param data A pointer to an array to be copied from the user.
2412 *
2413 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2414 */
inv_set_gyro_bias_float(float * data)2415 inv_error_t inv_set_gyro_bias_float(float *data)
2416 {
2417 long arrayTmp[3];
2418 arrayTmp[0] = (long)(data[0] * 65536.f);
2419 arrayTmp[1] = (long)(data[1] * 65536.f);
2420 arrayTmp[2] = (long)(data[2] * 65536.f);
2421 return inv_set_gyro_bias(arrayTmp);
2422
2423 }
2424
2425 /**
2426 * @brief inv_set_accel_bias_float is used to set the accelerometer bias.
2427 * The argument array elements are ordered X,Y,Z.
2428 * The values are in units of g (gravity).
2429 *
2430 * Please refer to the provided "9-Axis Sensor Fusion
2431 * Application Note" document provided.
2432 *
2433 * @pre MLDmpOpen() \ifnot UMPL or
2434 * MLDmpPedometerStandAloneOpen() \endif
2435 * @pre MLDmpStart() must <b>NOT</b> have been called.
2436 *
2437 * @param data A pointer to an array to be copied from the user.
2438 *
2439 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2440 */
inv_set_accel_bias_float(float * data)2441 inv_error_t inv_set_accel_bias_float(float *data)
2442 {
2443 long arrayTmp[3];
2444 arrayTmp[0] = (long)(data[0] * 65536.f);
2445 arrayTmp[1] = (long)(data[1] * 65536.f);
2446 arrayTmp[2] = (long)(data[2] * 65536.f);
2447 return inv_set_accel_bias(arrayTmp);
2448
2449 }
2450
2451 /**
2452 * @cond MPL
2453 * @brief inv_set_mag_bias_float is used to set compass bias
2454 *
2455 * Please refer to the provided "9-Axis Sensor Fusion
2456 * Application Note" document provided.
2457 *
2458 * @pre MLDmpOpen()\ifnot UMPL or
2459 * MLDmpPedometerStandAloneOpen() \endif
2460 * @pre MLDmpStart() must <b>NOT</b> have been called.
2461 *
2462 * @param data A pointer to an array to be copied from the user.
2463 *
2464 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2465 * @endcond
2466 */
inv_set_mag_bias_float(float * data)2467 inv_error_t inv_set_mag_bias_float(float *data)
2468 {
2469 long arrayTmp[3];
2470 arrayTmp[0] = (long)(data[0] * 65536.f);
2471 arrayTmp[1] = (long)(data[1] * 65536.f);
2472 arrayTmp[2] = (long)(data[2] * 65536.f);
2473 return inv_set_mag_bias(arrayTmp);
2474 }
2475
2476 /**
2477 * @cond MPL
2478 * @brief inv_set_local_field_float is used to set local magnetic field
2479 *
2480 * Please refer to the provided "9-Axis Sensor Fusion
2481 * Application Note" document provided.
2482 *
2483 * @pre MLDmpOpen() \ifnot UMPL or
2484 * MLDmpPedometerStandAloneOpen() \endif
2485 * @pre MLDmpStart() must <b>NOT</b> have been called.
2486 *
2487 * @param data A pointer to an array to be copied from the user.
2488 *
2489 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2490 * @endcond
2491 */
inv_set_local_field_float(float * data)2492 inv_error_t inv_set_local_field_float(float *data)
2493 {
2494 long arrayTmp[3];
2495 arrayTmp[0] = (long)(data[0] * 65536.f);
2496 arrayTmp[1] = (long)(data[1] * 65536.f);
2497 arrayTmp[2] = (long)(data[2] * 65536.f);
2498 return inv_set_local_field(arrayTmp);
2499 }
2500
2501 /**
2502 * @cond MPL
2503 * @brief inv_set_mag_scale_float is used to set magnetometer scale
2504 *
2505 * Please refer to the provided "9-Axis Sensor Fusion
2506 * Application Note" document provided.
2507 *
2508 * @pre MLDmpOpen() \ifnot UMPL or
2509 * MLDmpPedometerStandAloneOpen() \endif
2510 * @pre MLDmpStart() must <b>NOT</b> have been called.
2511 *
2512 * @param data A pointer to an array to be copied from the user.
2513 *
2514 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
2515 * @endcond
2516 */
inv_set_mag_scale_float(float * data)2517 inv_error_t inv_set_mag_scale_float(float *data)
2518 {
2519 long arrayTmp[3];
2520 arrayTmp[0] = (long)(data[0] * 65536.f);
2521 arrayTmp[1] = (long)(data[1] * 65536.f);
2522 arrayTmp[2] = (long)(data[2] * 65536.f);
2523 return inv_set_mag_scale(arrayTmp);
2524 }
2525