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_legacy.c $ 21 * 22 *****************************************************************************/ 23 24 /** 25 * @defgroup MLArray_Legacy 26 * @brief Legacy Motion Library Array APIs. 27 * The Motion Library Array APIs provide the user access to the 28 * Motion Library state. These Legacy APIs provide access to 29 * individual state arrays using a data set name as the first 30 * argument to the API. This format has been replaced by unique 31 * named APIs for each data set, found in the MLArray group. 32 * 33 * @{ 34 * @file mlarray_legacy.c 35 * @brief The Legacy Motion Library Array APIs. 36 */ 37 38 #include "ml.h" 39 #include "mltypes.h" 40 #include "mlinclude.h" 41 #include "mlFIFO.h" 42 #include "mldl_cfg.h" 43 44 /** 45 * @brief inv_get_array is used to get an array of processed motion sensor data. 46 * inv_get_array can be used to retrieve various data sets. Certain data 47 * sets require functions to be enabled using MLEnable in order to be 48 * valid. 49 * 50 * The available data sets are: 51 * 52 * - INV_ROTATION_MATRIX 53 * - INV_QUATERNION 54 * - INV_EULER_ANGLES_X 55 * - INV_EULER_ANGLES_Y 56 * - INV_EULER_ANGLES_Z 57 * - INV_EULER_ANGLES 58 * - INV_LINEAR_ACCELERATION 59 * - INV_LINEAR_ACCELERATION_WORLD 60 * - INV_GRAVITY 61 * - INV_ANGULAR_VELOCITY 62 * - INV_RAW_DATA 63 * - INV_GYROS 64 * - INV_ACCELS 65 * - INV_MAGNETOMETER 66 * - INV_GYRO_BIAS 67 * - INV_ACCEL_BIAS 68 * - INV_MAG_BIAS 69 * - INV_HEADING 70 * - INV_MAG_BIAS_ERROR 71 * - INV_PRESSURE 72 * 73 * Please refer to the documentation of inv_get_float_array() for a 74 * description of these data sets. 75 * 76 * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() 77 * must have been called. 78 * 79 * @param dataSet 80 * A constant specifying an array of data processed by the 81 * motion processor. 82 * @param data 83 * A pointer to an array to be passed back to the user. 84 * <b>Must be 9 cells long at least</b>. 85 * 86 * @return Zero if the command is successful; an ML error code otherwise. 87 */ 88 inv_error_t inv_get_array(int dataSet, long *data) 89 { 90 inv_error_t result; 91 switch (dataSet) { 92 case INV_GYROS: 93 result = inv_get_gyro(data); 94 break; 95 case INV_ACCELS: 96 result = inv_get_accel(data); 97 break; 98 case INV_TEMPERATURE: 99 result = inv_get_temperature(data); 100 break; 101 case INV_ROTATION_MATRIX: 102 result = inv_get_rot_mat(data); 103 break; 104 case INV_QUATERNION: 105 result = inv_get_quaternion(data); 106 break; 107 case INV_LINEAR_ACCELERATION: 108 result = inv_get_linear_accel(data); 109 break; 110 case INV_LINEAR_ACCELERATION_WORLD: 111 result = inv_get_linear_accel_in_world(data); 112 break; 113 case INV_GRAVITY: 114 result = inv_get_gravity(data); 115 break; 116 case INV_ANGULAR_VELOCITY: 117 result = inv_get_angular_velocity(data); 118 break; 119 case INV_EULER_ANGLES: 120 result = inv_get_euler_angles(data); 121 break; 122 case INV_EULER_ANGLES_X: 123 result = inv_get_euler_angles_x(data); 124 break; 125 case INV_EULER_ANGLES_Y: 126 result = inv_get_euler_angles_y(data); 127 break; 128 case INV_EULER_ANGLES_Z: 129 result = inv_get_euler_angles_z(data); 130 break; 131 case INV_GYRO_TEMP_SLOPE: 132 result = inv_get_gyro_temp_slope(data); 133 break; 134 case INV_GYRO_BIAS: 135 result = inv_get_gyro_bias(data); 136 break; 137 case INV_ACCEL_BIAS: 138 result = inv_get_accel_bias(data); 139 break; 140 case INV_MAG_BIAS: 141 result = inv_get_mag_bias(data); 142 break; 143 case INV_RAW_DATA: 144 result = inv_get_gyro_and_accel_sensor(data); 145 break; 146 case INV_MAG_RAW_DATA: 147 result = inv_get_mag_raw_data(data); 148 break; 149 case INV_MAGNETOMETER: 150 result = inv_get_magnetometer(data); 151 break; 152 case INV_PRESSURE: 153 result = inv_get_pressure(data); 154 break; 155 case INV_HEADING: 156 result = inv_get_heading(data); 157 break; 158 case INV_GYRO_CALIBRATION_MATRIX: 159 result = inv_get_gyro_cal_matrix(data); 160 break; 161 case INV_ACCEL_CALIBRATION_MATRIX: 162 result = inv_get_accel_cal_matrix(data); 163 break; 164 case INV_MAG_CALIBRATION_MATRIX: 165 result = inv_get_mag_cal_matrix(data); 166 break; 167 case INV_MAG_BIAS_ERROR: 168 result = inv_get_mag_bias_error(data); 169 break; 170 case INV_MAG_SCALE: 171 result = inv_get_mag_scale(data); 172 break; 173 case INV_LOCAL_FIELD: 174 result = inv_get_local_field(data); 175 break; 176 case INV_RELATIVE_QUATERNION: 177 result = inv_get_relative_quaternion(data); 178 break; 179 default: 180 return INV_ERROR_INVALID_PARAMETER; 181 break; 182 } 183 return result; 184 } 185 186 /** 187 * @brief inv_get_float_array is used to get an array of processed motion sensor 188 * data. inv_get_array can be used to retrieve various data sets. 189 * Certain data sets require functions to be enabled using MLEnable 190 * in order to be valid. 191 * 192 * The available data sets are: 193 * 194 * - INV_ROTATION_MATRIX : 195 * Returns an array of nine data points representing the rotation 196 * matrix generated from all available sensors. 197 * This requires that ML_SENSOR_FUSION be enabled. 198 * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, 199 * R33, representing the matrix: 200 * <center>R11 R12 R13</center> 201 * <center>R21 R22 R23</center> 202 * <center>R31 R32 R33</center> 203 * <b>Please refer to the "9-Axis Sensor Fusion Application Note" document, 204 * section 7 "Sensor Fusion Output", for details regarding rotation 205 * matrix output</b>. 206 * 207 * - INV_QUATERNION : 208 * Returns an array of four data points representing the quaternion 209 * generated from all available sensors. 210 * This requires that ML_SENSOR_FUSION be enabled. 211 * 212 * - INV_EULER_ANGLES_X : 213 * Returns an array of three data points representing roll, pitch, and 214 * yaw using the X axis of the gyroscope, accelerometer, and compass as 215 * reference axis. 216 * This is typically the convention used for mobile devices where the X 217 * axis is the width of the screen, Y axis is the height, and Z the 218 * depth. In this case roll is defined as the rotation around the X 219 * axis of the device. 220 * The euler angles convention for this output is the following: 221 * <TABLE> 222 * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> 223 * <TR><TD>roll </TD><TD>X axis </TD></TR> 224 * <TR><TD>pitch </TD><TD>Y axis </TD></TR> 225 * <TR><TD>yaw </TD><TD>Z axis </TD></TR> 226 * </TABLE> 227 * INV_EULER_ANGLES_X corresponds to the INV_EULER_ANGLES output and is 228 * therefore the default convention. 229 * 230 * - INV_EULER_ANGLES_Y : 231 * Returns an array of three data points representing roll, pitch, and 232 * yaw using the Y axis of the gyroscope, accelerometer, and compass as 233 * reference axis. 234 * This convention is typically used in augmented reality applications, 235 * where roll is defined as the rotation around the axis along the 236 * height of the screen of a mobile device, namely the Y axis. 237 * The euler angles convention for this output is the following: 238 * <TABLE> 239 * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> 240 * <TR><TD>roll </TD><TD>Y axis </TD></TR> 241 * <TR><TD>pitch </TD><TD>X axis </TD></TR> 242 * <TR><TD>yaw </TD><TD>Z axis </TD></TR> 243 * </TABLE> 244 * 245 * - INV_EULER_ANGLES_Z : 246 * Returns an array of three data points representing roll, pitch, and 247 * yaw using the Z axis of the gyroscope, accelerometer, and compass as 248 * reference axis. 249 * This convention is mostly used in application involving the use 250 * of a camera, typically placed on the back of a mobile device, that 251 * is along the Z axis. In this convention roll is defined as the 252 * rotation around the Z axis. 253 * The euler angles convention for this output is the following: 254 * <TABLE> 255 * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> 256 * <TR><TD>roll </TD><TD>Z axis </TD></TR> 257 * <TR><TD>pitch </TD><TD>X axis </TD></TR> 258 * <TR><TD>yaw </TD><TD>Y axis </TD></TR> 259 * </TABLE> 260 * 261 * - INV_EULER_ANGLES : 262 * Returns an array of three data points representing roll, pitch, and 263 * yaw corresponding to the INV_EULER_ANGLES_X output and it is 264 * therefore the default convention for Euler angles. 265 * Please refer to the INV_EULER_ANGLES_X for a detailed description. 266 * 267 * - INV_LINEAR_ACCELERATION : 268 * Returns an array of three data points representing the linear 269 * acceleration as derived from both gyroscopes and accelerometers. 270 * This requires that ML_SENSOR_FUSION be enabled. 271 * 272 * - INV_LINEAR_ACCELERATION_WORLD : 273 * Returns an array of three data points representing the linear 274 * acceleration in world coordinates, as derived from both gyroscopes 275 * and accelerometers. 276 * This requires that ML_SENSOR_FUSION be enabled. 277 * 278 * - INV_GRAVITY : 279 * Returns an array of three data points representing the direction 280 * of gravity in body coordinates, as derived from both gyroscopes 281 * and accelerometers. 282 * This requires that ML_SENSOR_FUSION be enabled. 283 * 284 * - INV_ANGULAR_VELOCITY : 285 * Returns an array of three data points representing the angular 286 * velocity as derived from <b>both</b> gyroscopes and accelerometers. 287 * This requires that ML_SENSOR_FUSION be enabled, to fuse data from 288 * the gyroscope and accelerometer device, appropriately scaled and 289 * oriented according to the respective mounting matrices. 290 * 291 * - INV_RAW_DATA : 292 * Returns an array of nine data points representing raw sensor data 293 * of the gyroscope X, Y, Z, accelerometer X, Y, Z, and 294 * compass X, Y, Z values. 295 * These values are not scaled and come out directly from the devices' 296 * sensor data output. In case of accelerometers with lower output 297 * resolution, e.g 8-bit, the sensor data is scaled up to match the 298 * 2^14 = 1 gee typical representation for a +/- 2 gee full scale 299 * range. 300 * 301 * - INV_GYROS : 302 * Returns an array of three data points representing the X gyroscope, 303 * Y gyroscope, and Z gyroscope values. 304 * The values are not sensor fused with other sensor types data but 305 * reflect the orientation from the mounting matrices in use. 306 * The INV_GYROS values are scaled to ensure 1 dps corresponds to 2^16 307 * codes. 308 * 309 * - INV_ACCELS : 310 * Returns an array of three data points representing the X 311 * accelerometer, Y accelerometer, and Z accelerometer values. 312 * The values are not sensor fused with other sensor types data but 313 * reflect the orientation from the mounting matrices in use. 314 * The INV_ACCELS values are scaled to ensure 1 gee corresponds to 2^16 315 * codes. 316 * 317 * - INV_MAGNETOMETER : 318 * Returns an array of three data points representing the compass 319 * X, Y, and Z values. 320 * The values are not sensor fused with other sensor types data but 321 * reflect the orientation from the mounting matrices in use. 322 * The INV_MAGNETOMETER values are scaled to ensure 1 micro Tesla (uT) 323 * corresponds to 2^16 codes. 324 * 325 * - INV_GYRO_BIAS : 326 * Returns an array of three data points representing the gyroscope 327 * biases. 328 * 329 * - INV_ACCEL_BIAS : 330 * Returns an array of three data points representing the 331 * accelerometer biases. 332 * 333 * - INV_MAG_BIAS : 334 * Returns an array of three data points representing the compass 335 * biases. 336 * 337 * - INV_GYRO_CALIBRATION_MATRIX : 338 * Returns an array of nine data points representing the calibration 339 * matrix for the gyroscopes: 340 * <center>C11 C12 C13</center> 341 * <center>C21 C22 C23</center> 342 * <center>C31 C32 C33</center> 343 * 344 * - INV_ACCEL_CALIBRATION_MATRIX : 345 * Returns an array of nine data points representing the calibration 346 * matrix for the accelerometers: 347 * <center>C11 C12 C13</center> 348 * <center>C21 C22 C23</center> 349 * <center>C31 C32 C33</center> 350 * 351 * - INV_MAG_CALIBRATION_MATRIX : 352 * Returns an array of nine data points representing the calibration 353 * matrix for the compass: 354 * <center>C11 C12 C13</center> 355 * <center>C21 C22 C23</center> 356 * <center>C31 C32 C33</center> 357 * 358 * - INV_PRESSURE : 359 * Returns a single value representing the pressure in Pascal 360 * 361 * - INV_HEADING : 362 * Returns a single number representing the heading of the device 363 * relative to the Earth, in which 0 represents North, 90 degrees 364 * represents East, and so on. 365 * The heading is defined as the direction of the +Y axis if the Y 366 * axis is horizontal, and otherwise the direction of the -Z axis. 367 * 368 * - INV_MAG_BIAS_ERROR : 369 * Returns an array of three numbers representing the current estimated 370 * error in the compass biases. These numbers are unitless and serve 371 * as rough estimates in which numbers less than 100 typically represent 372 * reasonably well calibrated compass axes. 373 * 374 * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() 375 * must have been called. 376 * 377 * @param dataSet 378 * A constant specifying an array of data processed by 379 * the motion processor. 380 * @param data 381 * A pointer to an array to be passed back to the user. 382 * <b>Must be 9 cells long at least</b>. 383 * 384 * @return INV_SUCCESS if the command is successful; an error code otherwise. 385 */ 386 inv_error_t inv_get_float_array(int dataSet, float *data) 387 { 388 inv_error_t result; 389 switch (dataSet) { 390 case INV_GYROS: 391 result = inv_get_gyro_float(data); 392 break; 393 case INV_ACCELS: 394 result = inv_get_accel_float(data); 395 break; 396 case INV_TEMPERATURE: 397 result = inv_get_temperature_float(data); 398 break; 399 case INV_ROTATION_MATRIX: 400 result = inv_get_rot_mat_float(data); 401 break; 402 case INV_QUATERNION: 403 result = inv_get_quaternion_float(data); 404 break; 405 case INV_LINEAR_ACCELERATION: 406 result = inv_get_linear_accel_float(data); 407 break; 408 case INV_LINEAR_ACCELERATION_WORLD: 409 result = inv_get_linear_accel_in_world_float(data); 410 break; 411 case INV_GRAVITY: 412 result = inv_get_gravity_float(data); 413 break; 414 case INV_ANGULAR_VELOCITY: 415 result = inv_get_angular_velocity_float(data); 416 break; 417 case INV_EULER_ANGLES: 418 result = inv_get_euler_angles_float(data); 419 break; 420 case INV_EULER_ANGLES_X: 421 result = inv_get_euler_angles_x_float(data); 422 break; 423 case INV_EULER_ANGLES_Y: 424 result = inv_get_euler_angles_y_float(data); 425 break; 426 case INV_EULER_ANGLES_Z: 427 result = inv_get_euler_angles_z_float(data); 428 break; 429 case INV_GYRO_TEMP_SLOPE: 430 result = inv_get_gyro_temp_slope_float(data); 431 break; 432 case INV_GYRO_BIAS: 433 result = inv_get_gyro_bias_float(data); 434 break; 435 case INV_ACCEL_BIAS: 436 result = inv_get_accel_bias_float(data); 437 break; 438 case INV_MAG_BIAS: 439 result = inv_get_mag_bias_float(data); 440 break; 441 case INV_RAW_DATA: 442 result = inv_get_gyro_and_accel_sensor_float(data); 443 break; 444 case INV_MAG_RAW_DATA: 445 result = inv_get_mag_raw_data_float(data); 446 break; 447 case INV_MAGNETOMETER: 448 result = inv_get_magnetometer_float(data); 449 break; 450 case INV_PRESSURE: 451 result = inv_get_pressure_float(data); 452 break; 453 case INV_HEADING: 454 result = inv_get_heading_float(data); 455 break; 456 case INV_GYRO_CALIBRATION_MATRIX: 457 result = inv_get_gyro_cal_matrix_float(data); 458 break; 459 case INV_ACCEL_CALIBRATION_MATRIX: 460 result = inv_get_accel_cal_matrix_float(data); 461 break; 462 case INV_MAG_CALIBRATION_MATRIX: 463 result = inv_get_mag_cal_matrix_float(data); 464 break; 465 case INV_MAG_BIAS_ERROR: 466 result = inv_get_mag_bias_error_float(data); 467 break; 468 case INV_MAG_SCALE: 469 result = inv_get_mag_scale_float(data); 470 break; 471 case INV_LOCAL_FIELD: 472 result = inv_get_local_field_float(data); 473 break; 474 case INV_RELATIVE_QUATERNION: 475 result = inv_get_relative_quaternion_float(data); 476 break; 477 default: 478 return INV_ERROR_INVALID_PARAMETER; 479 break; 480 } 481 return result; 482 } 483 484 /** 485 * @brief used to set an array of motion sensor data. 486 * Handles the following data sets: 487 * - INV_GYRO_BIAS 488 * - INV_ACCEL_BIAS 489 * - INV_MAG_BIAS 490 * - INV_GYRO_TEMP_SLOPE 491 * 492 * For more details about the use of the data sets 493 * please refer to the documentation of inv_set_float_array(). 494 * 495 * Please also refer to the provided "9-Axis Sensor Fusion 496 * Application Note" document provided. 497 * 498 * @pre MLDmpOpen() or 499 * MLDmpPedometerStandAloneOpen() 500 * @pre MLDmpStart() must <b>NOT</b> have been called. 501 * 502 * @param dataSet A constant specifying an array of data. 503 * @param data A pointer to an array to be copied from the user. 504 * 505 * @return INV_SUCCESS if successful; a non-zero error code otherwise. 506 */ 507 inv_error_t inv_set_array(int dataSet, long *data) 508 { 509 INVENSENSE_FUNC_START; 510 inv_error_t result; 511 switch (dataSet) { 512 case INV_GYRO_BIAS: 513 result = inv_set_gyro_bias(data); 514 break; 515 case INV_ACCEL_BIAS: 516 result = inv_set_accel_bias(data); 517 break; 518 case INV_MAG_BIAS: 519 result = inv_set_mag_bias(data); 520 break; 521 case INV_GYRO_TEMP_SLOPE: 522 result = inv_set_gyro_temp_slope(data); 523 break; 524 case INV_LOCAL_FIELD: 525 result = inv_set_local_field(data); 526 break; 527 case INV_MAG_SCALE: 528 result = inv_set_mag_scale(data); 529 break; 530 default: 531 return INV_ERROR_INVALID_PARAMETER; 532 break; 533 } 534 return result; 535 } 536 537 /** 538 * @brief used to set an array of motion sensor data. 539 * Handles various data sets: 540 * - INV_GYRO_BIAS 541 * - INV_ACCEL_BIAS 542 * - INV_MAG_BIAS 543 * - INV_GYRO_TEMP_SLOPE 544 * 545 * Please refer to the provided "9-Axis Sensor Fusion Application 546 * Note" document provided. 547 * 548 * @pre MLDmpOpen() or 549 * MLDmpPedometerStandAloneOpen() 550 * @pre MLDmpStart() must <b>NOT</b> have been called. 551 * 552 * @param dataSet A constant specifying an array of data. 553 * @param data A pointer to an array to be copied from the user. 554 * 555 * @return INV_SUCCESS if successful; a non-zero error code otherwise. 556 */ 557 inv_error_t inv_set_float_array(int dataSet, float *data) 558 { 559 INVENSENSE_FUNC_START; 560 inv_error_t result; 561 562 switch (dataSet) { 563 case INV_GYRO_TEMP_SLOPE: // internal 564 result = inv_set_gyro_temp_slope_float(data); 565 break; 566 case INV_GYRO_BIAS: // internal 567 result = inv_set_gyro_bias_float(data); 568 break; 569 case INV_ACCEL_BIAS: // internal 570 result = inv_set_accel_bias_float(data); 571 break; 572 case INV_MAG_BIAS: // internal 573 result = inv_set_mag_bias_float(data); 574 break; 575 case INV_LOCAL_FIELD: // internal 576 result = inv_set_local_field_float(data); 577 break; 578 case INV_MAG_SCALE: // internal 579 result = inv_set_mag_scale_float(data); 580 break; 581 default: 582 result = INV_ERROR_INVALID_PARAMETER; 583 break; 584 } 585 586 return result; 587 } 588
