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 */ 119inv_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 */ 217inv_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 */ 253inv_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 */ 284inv_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 */ 342inv_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 400inv_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 */ 451inv_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 */ 486inv_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 */ 516inv_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 */ 547inv_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 */ 609inv_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 */ 641inv_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 */ 672inv_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 702inv_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 */ 751inv_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 */ 796inv_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 */ 834inv_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 */ 872inv_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 */ 909inv_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 */ 941inv_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 */ 989inv_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 */ 1026inv_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 */ 1064inv_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 */ 1105inv_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 */ 1165inv_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 */ 1202inv_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 */ 1237inv_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 */ 1278inv_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 1324inv_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 */ 1368inv_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 */ 1412inv_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 */ 1452inv_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 */ 1486inv_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 */ 1520inv_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 */ 1572inv_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 */ 1625inv_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 */ 1673inv_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 */ 1700inv_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 */ 1762inv_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 */ 1808inv_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 */ 1845inv_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 */ 1878inv_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 */ 1913inv_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 */ 1963inv_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 */ 2003inv_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 */ 2037inv_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 */ 2071inv_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 */ 2104inv_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 */ 2128inv_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 */ 2175inv_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 */ 2239inv_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 */ 2279inv_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 */ 2323inv_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 */ 2356inv_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 */ 2390inv_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 */ 2415inv_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 */ 2441inv_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 */ 2467inv_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 */ 2492inv_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 */ 2517inv_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