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: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $ 21 * 22 *******************************************************************************/ 23 24 /** 25 * @defgroup MLFIFO 26 * @brief Motion Library - FIFO Driver. 27 * The FIFO API Interface. 28 * 29 * @{ 30 * @file mlFIFO.c 31 * @brief FIFO Interface. 32 **/ 33 34 #include <string.h> 35 #include "mpu.h" 36 #if defined CONFIG_MPU_SENSORS_MPU6050A2 37 # include "mpu6050a2.h" 38 #elif defined CONFIG_MPU_SENSORS_MPU6050B1 39 # include "mpu6050b1.h" 40 #elif defined CONFIG_MPU_SENSORS_MPU3050 41 # include "mpu3050.h" 42 #else 43 #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx 44 #endif 45 #include "mlFIFO.h" 46 #include "mlFIFOHW.h" 47 #include "dmpKey.h" 48 #include "mlMathFunc.h" 49 #include "ml.h" 50 #include "mldl.h" 51 #include "mldl_cfg.h" 52 #include "mlstates.h" 53 #include "mlsupervisor.h" 54 #include "mlos.h" 55 #include "mlmath.h" 56 #include "accel.h" 57 58 #include "log.h" 59 #undef MPL_LOG_TAG 60 #define MPL_LOG_TAG "MPL-fifo" 61 62 #define FIFO_DEBUG 0 63 64 #define REF_QUATERNION (0) 65 #define REF_GYROS (REF_QUATERNION + 4) 66 #define REF_CONTROL (REF_GYROS + 3) 67 #define REF_RAW (REF_CONTROL + 4) 68 #define REF_RAW_EXTERNAL (REF_RAW + 8) 69 #define REF_ACCEL (REF_RAW_EXTERNAL + 6) 70 #define REF_QUANT_ACCEL (REF_ACCEL + 3) 71 #define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) 72 #define REF_EIS (REF_QUATERNION_6AXIS + 4) 73 #define REF_DMP_PACKET (REF_EIS + 3) 74 #define REF_GARBAGE (REF_DMP_PACKET + 1) 75 #define REF_LAST (REF_GARBAGE + 1) 76 77 long fifo_scale[REF_LAST] = { 78 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 79 // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2) 80 1501974482L, 1501974482L, 1501974482L, // Gyro 81 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control 82 (1L << 14), // Temperature 83 (1L << 14), (1L << 14), (1L << 14), // Raw Gyro 84 (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding 85 (1L << 14), (1L << 14), (1L << 14), // Raw External 86 (1L << 14), (1L << 14), (1L << 14), // Raw External 87 (1L << 16), (1L << 16), (1L << 16), // Accel 88 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel 89 (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel 90 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis 91 (1L << 30), (1L << 30), (1L << 30), // EIS 92 (1L << 30), // Packet 93 (1L << 30), // Garbage 94 }; 95 96 // The scale factors for tap need to match the number in fifo_scale above. 97 // fifo_base_offset below may also need to be changed if this is not 8 98 #if INV_MAX_NUM_ACCEL_SAMPLES != 8 99 #error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8 100 #endif 101 102 #define CONFIG_QUAT (0) 103 #define CONFIG_GYROS (CONFIG_QUAT + 1) 104 #define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1) 105 #define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1) 106 #define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1) 107 #define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1) 108 #define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1) 109 #define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1) 110 #define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1) 111 #define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1) 112 #define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1) 113 #define NUMFIFOELEMENTS (CONFIG_FOOTER + 1) 114 115 const int fifo_base_offset[NUMFIFOELEMENTS] = { 116 REF_QUATERNION * 4, 117 REF_GYROS * 4, 118 REF_CONTROL * 4, 119 REF_RAW * 4, 120 REF_RAW * 4 + 4, 121 REF_RAW_EXTERNAL * 4, 122 REF_ACCEL * 4, 123 REF_QUANT_ACCEL * 4, 124 REF_EIS * 4, 125 REF_DMP_PACKET * 4, 126 REF_GARBAGE * 4 127 }; 128 129 struct fifo_obj { 130 void (*fifo_process_cb) (void); 131 long decoded[REF_LAST]; 132 long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES]; 133 int offsets[REF_LAST * 4]; 134 int cache; 135 uint_fast8_t gyro_source; 136 unsigned short fifo_rate; 137 unsigned short sample_step_size_ms; 138 uint_fast16_t fifo_packet_size; 139 uint_fast16_t data_config[NUMFIFOELEMENTS]; 140 unsigned char reference_count[REF_LAST]; 141 long acc_bias_filt[3]; 142 float acc_filter_coef; 143 long gravity_cache[3]; 144 }; 145 146 static struct fifo_obj fifo_obj; 147 148 #define FIFO_CACHE_TEMPERATURE 1 149 #define FIFO_CACHE_GYRO 2 150 #define FIFO_CACHE_GRAVITY_BODY 4 151 #define FIFO_CACHE_ACC_BIAS 8 152 153 struct fifo_rate_obj { 154 // These describe callbacks happening everytime a FIFO block is processed 155 int_fast8_t num_cb; 156 HANDLE mutex; 157 inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES]; 158 int priority[MAX_HIGH_RATE_PROCESSES]; 159 }; 160 161 struct fifo_rate_obj fifo_rate_obj; 162 163 /** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old 164 * accuracy if needed. 165 */ 166 static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements, 167 uint_fast16_t accuracy, 168 uint_fast8_t configOffset) 169 { 170 if (elements) { 171 if (!accuracy) 172 accuracy = fifo_obj.data_config[configOffset]; 173 else if (accuracy & INV_16_BIT) 174 if ((fifo_obj.data_config[configOffset] & INV_32_BIT)) 175 accuracy = INV_32_BIT; // 32-bits takes priority 176 else 177 accuracy = INV_16_BIT; 178 else 179 accuracy = INV_32_BIT; 180 } else { 181 accuracy = 0; 182 } 183 184 return accuracy; 185 } 186 187 /** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, 188 * the reference counts are subtracted, otherwise they are incremented for each 189 * bit set in element. The value returned are the elements that should be sent 190 * out as data through the FIFO. 191 */ 192 static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements, 193 uint_fast16_t increment, 194 uint_fast8_t refOffset, 195 uint_fast8_t len) 196 { 197 uint_fast8_t kk; 198 199 if (increment == 0) { 200 for (kk = 0; kk < len; ++kk) { 201 if ((elements & 1) 202 && (fifo_obj.reference_count[kk + refOffset] > 0)) { 203 fifo_obj.reference_count[kk + refOffset]--; 204 } 205 elements >>= 1; 206 } 207 } else { 208 for (kk = 0; kk < len; ++kk) { 209 if (elements & 1) 210 fifo_obj.reference_count[kk + refOffset]++; 211 elements >>= 1; 212 } 213 } 214 elements = 0; 215 for (kk = 0; kk < len; ++kk) { 216 if (fifo_obj.reference_count[kk + refOffset] > 0) 217 elements |= (1 << kk); 218 } 219 return elements; 220 } 221 222 /** 223 * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send 224 * out the FIFO, 0 when removing from the FIFO. 225 */ 226 static inv_error_t inv_construct3_fifo(unsigned char *regs, 227 uint_fast16_t elements, 228 uint_fast16_t accuracy, 229 uint_fast8_t refOffset, 230 unsigned short key, 231 uint_fast8_t configOffset) 232 { 233 int_fast8_t kk; 234 inv_error_t result; 235 236 elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3); 237 accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset); 238 239 if (accuracy & INV_16_BIT) { 240 regs[0] = DINAF8 + 2; 241 } 242 243 fifo_obj.data_config[configOffset] = elements | accuracy; 244 245 for (kk = 0; kk < 3; ++kk) { 246 if ((elements & 1) == 0) 247 regs[kk + 1] = DINAA0 + 3; 248 elements >>= 1; 249 } 250 251 result = inv_set_mpu_memory(key, 4, regs); 252 253 return result; 254 } 255 256 /** 257 * @internal 258 * Puts footer on FIFO data. 259 */ 260 static inv_error_t inv_set_footer(void) 261 { 262 unsigned char regs = DINA30; 263 uint_fast8_t tmp_count; 264 int_fast8_t i, j; 265 int offset; 266 int result; 267 int *fifo_offsets_ptr = fifo_obj.offsets; 268 269 fifo_obj.fifo_packet_size = 0; 270 for (i = 0; i < NUMFIFOELEMENTS; i++) { 271 tmp_count = 0; 272 offset = fifo_base_offset[i]; 273 for (j = 0; j < 8; j++) { 274 if ((fifo_obj.data_config[i] >> j) & 0x0001) { 275 #ifndef BIG_ENDIAN 276 // Special Case for Byte Ordering on Accel Data 277 if ((i == CONFIG_RAW_DATA) && (j > 2)) { 278 tmp_count += 2; 279 switch (inv_get_dl_config()->accel->endian) { 280 case EXT_SLAVE_BIG_ENDIAN: 281 *fifo_offsets_ptr++ = offset + 3; 282 *fifo_offsets_ptr++ = offset + 2; 283 break; 284 case EXT_SLAVE_LITTLE_ENDIAN: 285 *fifo_offsets_ptr++ = offset + 2; 286 *fifo_offsets_ptr++ = offset + 3; 287 break; 288 case EXT_SLAVE_FS8_BIG_ENDIAN: 289 if (j == 3) { 290 // Throw this byte away 291 *fifo_offsets_ptr++ = 292 fifo_base_offset[CONFIG_FOOTER]; 293 *fifo_offsets_ptr++ = offset + 3; 294 } else if (j == 4) { 295 *fifo_offsets_ptr++ = offset + 3; 296 *fifo_offsets_ptr++ = offset + 7; 297 } else { 298 // Throw these byte away 299 *fifo_offsets_ptr++ = 300 fifo_base_offset[CONFIG_FOOTER]; 301 *fifo_offsets_ptr++ = 302 fifo_base_offset[CONFIG_FOOTER]; 303 } 304 break; 305 case EXT_SLAVE_FS16_BIG_ENDIAN: 306 if (j == 3) { 307 // Throw this byte away 308 *fifo_offsets_ptr++ = 309 fifo_base_offset[CONFIG_FOOTER]; 310 *fifo_offsets_ptr++ = offset + 3; 311 } else if (j == 4) { 312 *fifo_offsets_ptr++ = offset - 2; 313 *fifo_offsets_ptr++ = offset + 3; 314 } else { 315 *fifo_offsets_ptr++ = offset - 2; 316 *fifo_offsets_ptr++ = offset + 3; 317 } 318 break; 319 default: 320 return INV_ERROR; // Bad value on ordering 321 } 322 } else { 323 tmp_count += 2; 324 *fifo_offsets_ptr++ = offset + 3; 325 *fifo_offsets_ptr++ = offset + 2; 326 if (fifo_obj.data_config[i] & INV_32_BIT) { 327 *fifo_offsets_ptr++ = offset + 1; 328 *fifo_offsets_ptr++ = offset; 329 tmp_count += 2; 330 } 331 } 332 #else 333 // Big Endian Platform 334 // Special Case for Byte Ordering on Accel Data 335 if ((i == CONFIG_RAW_DATA) && (j > 2)) { 336 tmp_count += 2; 337 switch (inv_get_dl_config()->accel->endian) { 338 case EXT_SLAVE_BIG_ENDIAN: 339 *fifo_offsets_ptr++ = offset + 2; 340 *fifo_offsets_ptr++ = offset + 3; 341 break; 342 case EXT_SLAVE_LITTLE_ENDIAN: 343 *fifo_offsets_ptr++ = offset + 3; 344 *fifo_offsets_ptr++ = offset + 2; 345 break; 346 case EXT_SLAVE_FS8_BIG_ENDIAN: 347 if (j == 3) { 348 // Throw this byte away 349 *fifo_offsets_ptr++ = 350 fifo_base_offset[CONFIG_FOOTER]; 351 *fifo_offsets_ptr++ = offset; 352 } else if (j == 4) { 353 *fifo_offsets_ptr++ = offset; 354 *fifo_offsets_ptr++ = offset + 4; 355 } else { 356 // Throw these bytes away 357 *fifo_offsets_ptr++ = 358 fifo_base_offset[CONFIG_FOOTER]; 359 *fifo_offsets_ptr++ = 360 fifo_base_offset[CONFIG_FOOTER]; 361 } 362 break; 363 case EXT_SLAVE_FS16_BIG_ENDIAN: 364 if (j == 3) { 365 // Throw this byte away 366 *fifo_offsets_ptr++ = 367 fifo_base_offset[CONFIG_FOOTER]; 368 *fifo_offsets_ptr++ = offset; 369 } else if (j == 4) { 370 *fifo_offsets_ptr++ = offset - 3; 371 *fifo_offsets_ptr++ = offset; 372 } else { 373 *fifo_offsets_ptr++ = offset - 3; 374 *fifo_offsets_ptr++ = offset; 375 } 376 break; 377 default: 378 return INV_ERROR; // Bad value on ordering 379 } 380 } else { 381 tmp_count += 2; 382 *fifo_offsets_ptr++ = offset; 383 *fifo_offsets_ptr++ = offset + 1; 384 if (fifo_obj.data_config[i] & INV_32_BIT) { 385 *fifo_offsets_ptr++ = offset + 2; 386 *fifo_offsets_ptr++ = offset + 3; 387 tmp_count += 2; 388 } 389 } 390 391 #endif 392 } 393 offset += 4; 394 } 395 fifo_obj.fifo_packet_size += tmp_count; 396 } 397 if (fifo_obj.data_config[CONFIG_FOOTER] == 0 && 398 fifo_obj.fifo_packet_size > 0) { 399 // Add footer 400 result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); 401 if (result) { 402 LOG_RESULT_LOCATION(result); 403 return result; 404 } 405 fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT; 406 fifo_obj.fifo_packet_size += 2; 407 } else if (fifo_obj.data_config[CONFIG_FOOTER] && 408 (fifo_obj.fifo_packet_size == 2)) { 409 // Remove Footer 410 regs = DINAA0 + 3; 411 result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); 412 if (result) { 413 LOG_RESULT_LOCATION(result); 414 return result; 415 } 416 fifo_obj.data_config[CONFIG_FOOTER] = 0; 417 fifo_obj.fifo_packet_size = 0; 418 } 419 420 return INV_SUCCESS; 421 } 422 423 inv_error_t inv_decode_quantized_accel(void) 424 { 425 int kk; 426 int fifoRate = inv_get_fifo_rate(); 427 428 if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) 429 return INV_ERROR_FEATURE_NOT_ENABLED; 430 431 for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1)); 432 kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) { 433 union { 434 unsigned int u10:10; 435 signed int s10:10; 436 } temp; 437 438 union { 439 uint32_t u32; 440 int32_t s32; 441 } value; 442 443 value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk]; 444 // unquantize this samples. 445 // They are stored as x * 2^20 + y * 2^10 + z 446 // Z 447 temp.u10 = value.u32 & 0x3ff; 448 value.s32 -= temp.s10; 449 fifo_obj.decoded_accel[kk][2] = temp.s10 * 64; 450 // Y 451 value.s32 = value.s32 / 1024; 452 temp.u10 = value.u32 & 0x3ff; 453 value.s32 -= temp.s10; 454 fifo_obj.decoded_accel[kk][1] = temp.s10 * 64; 455 // X 456 value.s32 = value.s32 / 1024; 457 temp.u10 = value.u32 & 0x3ff; 458 fifo_obj.decoded_accel[kk][0] = temp.s10 * 64; 459 } 460 return INV_SUCCESS; 461 } 462 463 static inv_error_t inv_state_change_fifo(unsigned char newState) 464 { 465 inv_error_t result = INV_SUCCESS; 466 unsigned char regs[4]; 467 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 468 469 /* Don't reset the fifo on a fifo rate change */ 470 if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && 471 (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) { 472 /* Delay output on restart by 50ms due to warm up time of gyros */ 473 474 short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1); 475 inv_init_fifo_hardare(); 476 inv_int16_to_big8(delay, regs); 477 result = inv_set_mpu_memory(KEY_D_1_178, 2, regs); 478 if (result) { 479 LOG_RESULT_LOCATION(result); 480 return result; 481 } 482 } 483 484 if (INV_STATE_DMP_STARTED == newState) { 485 if (inv_dmpkey_supported(KEY_D_1_128)) { 486 double tmp; 487 tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1); 488 if (tmp > 0x40000000L) 489 tmp = 0x40000000L; 490 (void)inv_int32_to_big8((long)tmp, regs); 491 result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs); 492 if (result) { 493 LOG_RESULT_LOCATION(result); 494 return result; 495 } 496 result = inv_reset_fifo(); 497 if (result) { 498 LOG_RESULT_LOCATION(result); 499 return result; 500 } 501 } 502 } 503 return result; 504 } 505 506 /** 507 * @internal 508 * @brief get the FIFO packet size 509 * @return the FIFO packet size 510 */ 511 uint_fast16_t inv_get_fifo_packet_size(void) 512 { 513 return fifo_obj.fifo_packet_size; 514 } 515 516 /** 517 * @brief Initializes all the internal static variables for 518 * the FIFO module. 519 * @note Should be called by the initialization routine such 520 * as inv_dmp_open(). 521 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 522 */ 523 inv_error_t inv_init_fifo_param(void) 524 { 525 inv_error_t result; 526 memset(&fifo_obj, 0, sizeof(struct fifo_obj)); 527 fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity 528 inv_set_linear_accel_filter_coef(0.f); 529 fifo_obj.fifo_rate = 20; 530 fifo_obj.sample_step_size_ms = 100; 531 memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); 532 result = inv_create_mutex(&fifo_rate_obj.mutex); 533 if (result) { 534 LOG_RESULT_LOCATION(result); 535 return result; 536 } 537 result = inv_register_state_callback(inv_state_change_fifo); 538 if (result) { 539 LOG_RESULT_LOCATION(result); 540 return result; 541 } 542 return result; 543 } 544 545 /** 546 * @brief Close the FIFO usage. 547 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 548 */ 549 inv_error_t inv_close_fifo(void) 550 { 551 inv_error_t result; 552 inv_error_t first = INV_SUCCESS; 553 result = inv_unregister_state_callback(inv_state_change_fifo); 554 ERROR_CHECK_FIRST(first, result); 555 result = inv_destroy_mutex(fifo_rate_obj.mutex); 556 ERROR_CHECK_FIRST(first, result); 557 memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); 558 return first; 559 } 560 561 /** 562 * Set the gyro source to output to the fifo 563 * 564 * @param source The source. One of 565 * - INV_GYRO_FROM_RAW 566 * - INV_GYRO_FROM_QUATERNION 567 * 568 * @return INV_SUCCESS or non-zero error code; 569 */ 570 inv_error_t inv_set_gyro_data_source(uint_fast8_t source) 571 { 572 if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) { 573 return INV_ERROR_INVALID_PARAMETER; 574 } 575 576 fifo_obj.gyro_source = source; 577 return INV_SUCCESS; 578 } 579 580 /** 581 * @brief Reads and processes FIFO data. Also handles callbacks when data is 582 * ready. 583 * @param numPackets 584 * Number of FIFO packets to try to read. You should 585 * use a large number here, such as 100, if you want to read all 586 * the full packets in the FIFO, which is typical operation. 587 * @param processed 588 * The number of FIFO packets processed. This may be incremented 589 * even if high rate processes later fail. 590 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 591 */ 592 inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, 593 int_fast8_t * processed) 594 { 595 int_fast8_t packet; 596 inv_error_t result = INV_SUCCESS; 597 uint_fast16_t read; 598 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 599 int kk; 600 601 if (NULL == processed) 602 return INV_ERROR_INVALID_PARAMETER; 603 604 *processed = 0; 605 if (fifo_obj.fifo_packet_size == 0) 606 return result; // Nothing to read 607 608 for (packet = 0; packet < numPackets; ++packet) { 609 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { 610 unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE]; 611 unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE]; 612 read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size, 613 footer_n_data); 614 if (0 == read || 615 read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) { 616 result = inv_get_fifo_status(); 617 if (INV_SUCCESS != result) { 618 memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); 619 } 620 return result; 621 } 622 623 result = inv_process_fifo_packet(buf); 624 if (result) { 625 LOG_RESULT_LOCATION(result); 626 return result; 627 } 628 } else if (inv_accel_present()) { 629 long data[ACCEL_NUM_AXES]; 630 result = inv_get_accel_data(data); 631 if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { 632 return INV_SUCCESS; 633 } 634 if (result) { 635 LOG_RESULT_LOCATION(result); 636 return result; 637 } 638 639 memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); 640 fifo_obj.cache = 0; 641 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 642 fifo_obj.decoded[REF_RAW + 4 + kk] = 643 inv_q30_mult((data[kk] << 16), 644 fifo_scale[REF_RAW + 4 + kk]); 645 fifo_obj.decoded[REF_ACCEL + kk] = 646 inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]); 647 fifo_obj.decoded[REF_ACCEL + kk] -= 648 inv_obj.scaled_accel_bias[kk]; 649 } 650 } 651 // The buffer was processed correctly, so increment even if 652 // other processes fail later, which will return an error 653 *processed = *processed + 1; 654 655 if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) && 656 fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) { 657 result = inv_decode_quantized_accel(); 658 if (result) { 659 LOG_RESULT_LOCATION(result); 660 return result; 661 } 662 } 663 664 if (fifo_obj.data_config[CONFIG_QUAT]) { 665 result = inv_accel_compass_supervisor(); 666 if (result) { 667 LOG_RESULT_LOCATION(result); 668 return result; 669 } 670 } 671 672 result = inv_pressure_supervisor(); 673 if (result) { 674 LOG_RESULT_LOCATION(result); 675 return result; 676 } 677 678 // Callbacks now that we have a buffer of data ready 679 result = inv_run_fifo_rate_processes(); 680 if (result) { 681 LOG_RESULT_LOCATION(result); 682 return result; 683 } 684 685 } 686 return result; 687 } 688 689 /** 690 * @brief inv_set_fifo_processed_callback is used to set a processed data callback 691 * function. inv_set_fifo_processed_callback sets a user defined callback 692 * function that triggers when all the decoding has been finished by 693 * the motion processing engines. It is called before other bigger 694 * processing engines to allow lower latency for the user. 695 * 696 * @pre inv_dmp_open() 697 * @ifnot MPL_MF 698 * or inv_open_low_power_pedometer() 699 * or inv_eis_open_dmp() 700 * @endif 701 * and inv_dmp_start() 702 * must <b>NOT</b> have been called. 703 * 704 * @param func A user defined callback function. 705 * 706 * @return INV_SUCCESS if successful, or non-zero error code otherwise. 707 */ 708 inv_error_t inv_set_fifo_processed_callback(void (*func) (void)) 709 { 710 INVENSENSE_FUNC_START; 711 712 if (inv_get_state() < INV_STATE_DMP_OPENED) 713 return INV_ERROR_SM_IMPROPER_STATE; 714 715 fifo_obj.fifo_process_cb = func; 716 717 return INV_SUCCESS; 718 } 719 720 /** 721 * @internal 722 * @brief Process data from the dmp read via the fifo. Takes a buffer 723 * filled with bytes read from the DMP FIFO. 724 * Currently expects only the 16 bytes of quaternion data. 725 * Calculates the motion parameters from that data and stores the 726 * results in an internal data structure. 727 * @param[in,out] dmpData Pointer to the buffer containing the fifo data. 728 * @return INV_SUCCESS or error code. 729 **/ 730 inv_error_t inv_process_fifo_packet(const unsigned char *dmpData) 731 { 732 INVENSENSE_FUNC_START; 733 int N, kk; 734 unsigned char *p; 735 736 p = (unsigned char *)(&fifo_obj.decoded); 737 N = fifo_obj.fifo_packet_size; 738 if (N > sizeof(fifo_obj.decoded)) 739 return INV_ERROR_ASSERTION_FAILURE; 740 741 memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); 742 743 for (kk = 0; kk < N; ++kk) { 744 p[fifo_obj.offsets[kk]] = *dmpData++; 745 } 746 747 // If multiplies are much greater cost than if checks, you could check 748 // to see if fifo_scale is non-zero first, or equal to (1L<<30) 749 for (kk = 0; kk < REF_LAST; ++kk) { 750 fifo_obj.decoded[kk] = 751 inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]); 752 } 753 754 memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS], 755 &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long)); 756 757 inv_obj.flags[INV_PROCESSED_DATA_READY] = 1; 758 fifo_obj.cache = 0; 759 760 return INV_SUCCESS; 761 } 762 763 /** Converts 16-bit temperature data as read from temperature register 764 * into Celcius scaled by 2^16. 765 */ 766 long inv_decode_temperature(short tempReg) 767 { 768 #if defined CONFIG_MPU_SENSORS_MPU6050A2 769 // Celcius = 35 + (T + 3048.7)/325.9 770 return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L); 771 #endif 772 #if defined CONFIG_MPU_SENSORS_MPU6050B1 773 // Celcius = 35 + (T + 927.4)/360.6 774 return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L); 775 #endif 776 #if defined CONFIG_MPU_SENSORS_MPU3050 777 // Celcius = 35 + (T + 13200)/280 778 return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L); 779 #endif 780 } 781 782 /** @internal 783 * Returns the temperature in hardware units. The scaling may change from part to part. 784 */ 785 inv_error_t inv_get_temperature_raw(short *data) 786 { 787 if (data == NULL) 788 return INV_ERROR_INVALID_PARAMETER; 789 790 if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) { 791 inv_error_t result; 792 unsigned char regs[2]; 793 if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) { 794 if (FIFO_DEBUG) 795 MPL_LOGI("Fetching the temperature from the registers\n"); 796 fifo_obj.cache |= FIFO_CACHE_TEMPERATURE; 797 result = inv_serial_read(inv_get_serial_handle(), 798 inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2, 799 regs); 800 if (result) { 801 LOG_RESULT_LOCATION(result); 802 return result; 803 } 804 fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]); 805 } 806 } 807 *data = (short)fifo_obj.decoded[REF_RAW]; 808 return INV_SUCCESS; 809 } 810 811 /** 812 * @brief Returns 1-element vector of temperature. It is read from the hardware if it 813 * doesn't exist in the FIFO. 814 * @param[out] data 1-element vector of temperature 815 * @return 0 on success or an error code. 816 */ 817 inv_error_t inv_get_temperature(long *data) 818 { 819 short tr; 820 inv_error_t result; 821 822 if (data == NULL) 823 return INV_ERROR_INVALID_PARAMETER; 824 result = inv_get_temperature_raw(&tr); 825 if (result) { 826 LOG_RESULT_LOCATION(result); 827 return result; 828 } 829 data[0] = inv_decode_temperature(tr); 830 return INV_SUCCESS; 831 } 832 833 /** 834 * @brief Get the Decoded Accel Data. 835 * @param data 836 * a buffer to store the quantized data. 837 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 838 */ 839 inv_error_t inv_get_unquantized_accel(long *data) 840 { 841 int ii, kk; 842 if (data == NULL) 843 return INV_ERROR_INVALID_PARAMETER; 844 845 if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) 846 return INV_ERROR_FEATURE_NOT_ENABLED; 847 848 for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { 849 for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { 850 data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk]; 851 } 852 } 853 854 return INV_SUCCESS; 855 } 856 857 /** 858 * @brief Get the Quantized Accel data algorithm output from the FIFO. 859 * @param data 860 * a buffer to store the quantized data. 861 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 862 */ 863 inv_error_t inv_get_quantized_accel(long *data) 864 { 865 int ii; 866 if (data == NULL) 867 return INV_ERROR_INVALID_PARAMETER; 868 869 if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) 870 return INV_ERROR_FEATURE_NOT_ENABLED; 871 872 for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { 873 data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii]; 874 } 875 876 return INV_SUCCESS; 877 } 878 879 /** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO 880 * and it is read from the registers if it was not put into the FIFO. The data is 881 * cached till the next FIFO processing block time. 882 * @param[out] data Length 3, Gyro data 883 */ 884 inv_error_t inv_get_gyro_sensor(long *data) 885 { 886 if (data == NULL) 887 return INV_ERROR_INVALID_PARAMETER; 888 if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) { 889 inv_error_t result; 890 unsigned char regs[6]; 891 if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) { 892 fifo_obj.cache |= FIFO_CACHE_GYRO; 893 result = 894 inv_serial_read(inv_get_serial_handle(), 895 inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6, 896 regs); 897 if (result) { 898 LOG_RESULT_LOCATION(result); 899 return result; 900 } 901 fifo_obj.decoded[REF_RAW + 1] = 902 (((long)regs[0]) << 24) | (((long)regs[1]) << 16); 903 fifo_obj.decoded[REF_RAW + 2] = 904 (((long)regs[2]) << 24) | (((long)regs[3]) << 16); 905 fifo_obj.decoded[REF_RAW + 3] = 906 (((long)regs[4]) << 24) | (((long)regs[5]) << 16); 907 908 // Temperature starts at location 0, Gyro at location 1. 909 fifo_obj.decoded[REF_RAW + 1] = 910 inv_q30_mult(fifo_obj.decoded[REF_RAW + 1], 911 fifo_scale[REF_RAW + 1]); 912 fifo_obj.decoded[REF_RAW + 2] = 913 inv_q30_mult(fifo_obj.decoded[REF_RAW + 2], 914 fifo_scale[REF_RAW + 2]); 915 fifo_obj.decoded[REF_RAW + 3] = 916 inv_q30_mult(fifo_obj.decoded[REF_RAW + 3], 917 fifo_scale[REF_RAW + 3]); 918 } 919 data[0] = fifo_obj.decoded[REF_RAW + 1]; 920 data[1] = fifo_obj.decoded[REF_RAW + 2]; 921 data[2] = fifo_obj.decoded[REF_RAW + 3]; 922 } else { 923 long data2[6]; 924 inv_get_gyro_and_accel_sensor(data2); 925 data[0] = data2[0]; 926 data[1] = data2[1]; 927 data[2] = data2[2]; 928 } 929 return INV_SUCCESS; 930 } 931 932 /** 933 * @brief Returns 6-element vector of gyro and accel data 934 * @param[out] data 6-element vector of gyro and accel data 935 * @return 0 on success or an error code. 936 */ 937 inv_error_t inv_get_gyro_and_accel_sensor(long *data) 938 { 939 int ii; 940 if (data == NULL) 941 return INV_ERROR_INVALID_PARAMETER; 942 943 if (!fifo_obj.data_config[CONFIG_RAW_DATA]) 944 return INV_ERROR_FEATURE_NOT_ENABLED; 945 946 for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) { 947 data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii]; 948 } 949 950 return INV_SUCCESS; 951 } 952 953 /** 954 * @brief Returns 3-element vector of external sensor 955 * @param[out] data 3-element vector of external sensor 956 * @return 0 on success or an error code. 957 */ 958 inv_error_t inv_get_external_sensor_data(long *data, int size) 959 { 960 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 961 defined CONFIG_MPU_SENSORS_MPU6050B1 962 int ii; 963 if (data == NULL) 964 return INV_ERROR_INVALID_PARAMETER; 965 966 if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL]) 967 return INV_ERROR_FEATURE_NOT_ENABLED; 968 969 for (ii = 0; ii < size && ii < 6; ii++) { 970 data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii]; 971 } 972 973 return INV_SUCCESS; 974 #else 975 memset(data, 0, COMPASS_NUM_AXES * sizeof(long)); 976 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 977 #endif 978 } 979 980 /** 981 * Sends accelerometer data to the FIFO. 982 * 983 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis 984 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 985 * for a subset. 986 * 987 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 988 * bit data. Set to zero to remove it from the FIFO. 989 */ 990 inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy) 991 { 992 INVENSENSE_FUNC_START; 993 unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 }; 994 inv_error_t result; 995 int kk; 996 997 if (inv_get_state() < INV_STATE_DMP_OPENED) 998 return INV_ERROR_SM_IMPROPER_STATE; 999 1000 result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL, 1001 KEY_CFG_12, CONFIG_ACCEL); 1002 if (result) { 1003 LOG_RESULT_LOCATION(result); 1004 return result; 1005 } 1006 1007 for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { 1008 fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens; 1009 } 1010 1011 return inv_set_footer(); 1012 } 1013 1014 /** 1015 * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits. 1016 * 1017 * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all 1018 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd 1019 * together for a subset. 1020 * 1021 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1022 * bit data. Set to zero to remove it from the FIFO. 1023 */ 1024 inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy) 1025 { 1026 INVENSENSE_FUNC_START; 1027 int_fast8_t kk; 1028 inv_error_t result; 1029 unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; 1030 1031 if (inv_get_state() < INV_STATE_DMP_OPENED) 1032 return INV_ERROR_SM_IMPROPER_STATE; 1033 1034 elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4); 1035 accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA); 1036 1037 if (accuracy & INV_16_BIT) { 1038 regs[0] = DINAF8 + 2; 1039 } 1040 1041 fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy; 1042 1043 for (kk = 0; kk < 4; ++kk) { 1044 if ((elements & 1) == 0) 1045 regs[kk + 1] = DINAA0 + 3; 1046 elements >>= 1; 1047 } 1048 1049 result = inv_set_mpu_memory(KEY_CFG_1, 5, regs); 1050 if (result) { 1051 LOG_RESULT_LOCATION(result); 1052 return result; 1053 } 1054 1055 return inv_set_footer(); 1056 } 1057 1058 /** 1059 * Adds a rolling counter to the fifo packet. When used with the footer 1060 * the data comes out the first time: 1061 * 1062 * @code 1063 * <data0><data1>...<dataN><PacketNum0><PacketNum1> 1064 * @endcode 1065 * for every other packet it is 1066 * 1067 * @code 1068 * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1> 1069 * @endcode 1070 * 1071 * This allows for scanning of the fifo for packets 1072 * 1073 * @return INV_SUCCESS or error code 1074 */ 1075 inv_error_t inv_send_packet_number(uint_fast16_t accuracy) 1076 { 1077 INVENSENSE_FUNC_START; 1078 inv_error_t result; 1079 unsigned char regs; 1080 uint_fast16_t elements; 1081 1082 if (inv_get_state() < INV_STATE_DMP_OPENED) 1083 return INV_ERROR_SM_IMPROPER_STATE; 1084 1085 elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1); 1086 if (elements & 1) { 1087 regs = DINA28; 1088 fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 1089 INV_ELEMENT_1 | INV_16_BIT; 1090 } else { 1091 regs = DINAF8 + 3; 1092 fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0; 1093 } 1094 result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s); 1095 if (result) { 1096 LOG_RESULT_LOCATION(result); 1097 return result; 1098 } 1099 1100 return inv_set_footer(); 1101 } 1102 1103 /** 1104 * @brief Send the computed gravity vectors into the FIFO. 1105 * The gravity vectors can be retrieved from the FIFO via 1106 * inv_get_gravity(), to have the gravitation vector expressed 1107 * in coordinates relative to the body. 1108 * 1109 * Gravity is a derived vector derived from the quaternion. 1110 * @param elements 1111 * the gravitation vectors components bitmask. 1112 * To send all compoents use INV_ALL. 1113 * @param accuracy 1114 * The number of bits the gravitation vector is expressed 1115 * into. 1116 * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1117 * bit data. 1118 * Set to zero to remove it from the FIFO. 1119 * 1120 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1121 */ 1122 inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy) 1123 { 1124 INVENSENSE_FUNC_START; 1125 inv_error_t result; 1126 1127 result = inv_send_quaternion(accuracy); 1128 if (result) { 1129 LOG_RESULT_LOCATION(result); 1130 return result; 1131 } 1132 1133 return inv_set_footer(); 1134 } 1135 1136 /** Sends gyro data to the FIFO. Gyro data is a 3 length vector 1137 * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). 1138 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them 1139 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1140 * for a subset. 1141 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1142 * bit data. Set to zero to remove it from the FIFO. 1143 */ 1144 inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy) 1145 { 1146 INVENSENSE_FUNC_START; 1147 unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 }; 1148 inv_error_t result; 1149 1150 if (inv_get_state() < INV_STATE_DMP_OPENED) 1151 return INV_ERROR_SM_IMPROPER_STATE; 1152 1153 if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) { 1154 regs[0] = DINA90 + 5; 1155 result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); 1156 if (result) { 1157 LOG_RESULT_LOCATION(result); 1158 return result; 1159 } 1160 regs[0] = DINAF8 + 1; 1161 regs[1] = DINA20; 1162 regs[2] = DINA28; 1163 regs[3] = DINA30; 1164 } else { 1165 regs[0] = DINA90 + 10; 1166 result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); 1167 if (result) { 1168 LOG_RESULT_LOCATION(result); 1169 return result; 1170 } 1171 regs[0] = DINAF8 + 1; 1172 regs[1] = DINA28; 1173 regs[2] = DINA30; 1174 regs[3] = DINA38; 1175 } 1176 result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS, 1177 KEY_CFG_9, CONFIG_GYROS); 1178 1179 return inv_set_footer(); 1180 } 1181 1182 /** Sends linear accelerometer data to the FIFO. 1183 * 1184 * Linear accelerometer data is a 3 length vector of 32-bits. It is the 1185 * acceleration in the body frame with gravity removed. 1186 * 1187 * 1188 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of 1189 * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1190 * for a subset. 1191 * 1192 * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES 1193 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1194 * bit data. Set to zero to remove it from the FIFO. 1195 */ 1196 inv_error_t inv_send_linear_accel(uint_fast16_t elements, 1197 uint_fast16_t accuracy) 1198 { 1199 INVENSENSE_FUNC_START; 1200 inv_error_t result; 1201 unsigned char state = inv_get_state(); 1202 1203 if (state < INV_STATE_DMP_OPENED) 1204 return INV_ERROR_SM_IMPROPER_STATE; 1205 1206 result = inv_send_gravity(elements, accuracy); 1207 if (result) { 1208 LOG_RESULT_LOCATION(result); 1209 return result; 1210 } 1211 result = inv_send_accel(elements, accuracy); 1212 if (result) { 1213 LOG_RESULT_LOCATION(result); 1214 return result; 1215 } 1216 1217 return inv_set_footer(); 1218 } 1219 1220 /** Sends linear world accelerometer data to the FIFO. Linear world 1221 * accelerometer data is a 3 length vector of 32-bits. It is the acceleration 1222 * in the world frame with gravity removed. Should be called once after 1223 * inv_dmp_open() and before inv_dmp_start(). 1224 * 1225 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of 1226 * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1227 * for a subset. 1228 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1229 * bit data. 1230 */ 1231 inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, 1232 uint_fast16_t accuracy) 1233 { 1234 INVENSENSE_FUNC_START; 1235 inv_error_t result; 1236 1237 result = inv_send_linear_accel(INV_ALL, accuracy); 1238 if (result) { 1239 LOG_RESULT_LOCATION(result); 1240 return result; 1241 } 1242 result = inv_send_quaternion(accuracy); 1243 1244 return inv_set_footer(); 1245 } 1246 1247 /** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector 1248 * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). 1249 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1250 * bit data. 1251 */ 1252 inv_error_t inv_send_quaternion(uint_fast16_t accuracy) 1253 { 1254 INVENSENSE_FUNC_START; 1255 unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, 1256 DINA30, DINA38 1257 }; 1258 uint_fast16_t elements, kk; 1259 inv_error_t result; 1260 1261 if (inv_get_state() < INV_STATE_DMP_OPENED) 1262 return INV_ERROR_SM_IMPROPER_STATE; 1263 1264 elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4); 1265 accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT); 1266 1267 if (accuracy & INV_16_BIT) { 1268 regs[0] = DINAF8 + 2; 1269 } 1270 1271 fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy; 1272 1273 for (kk = 0; kk < 4; ++kk) { 1274 if ((elements & 1) == 0) 1275 regs[kk + 1] = DINAA0 + 3; 1276 elements >>= 1; 1277 } 1278 1279 result = inv_set_mpu_memory(KEY_CFG_8, 5, regs); 1280 if (result) { 1281 LOG_RESULT_LOCATION(result); 1282 return result; 1283 } 1284 1285 return inv_set_footer(); 1286 } 1287 1288 /** Sends raw data to the FIFO. 1289 * Should be called once after inv_dmp_open() and before inv_dmp_start(). 1290 * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them 1291 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together 1292 * for a subset. The first element is temperature, the next 3 are gyro data, 1293 * and the last 3 accel data. 1294 * @param accuracy 1295 * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off. 1296 * @return 0 if successful, a non-zero error code otherwise. 1297 */ 1298 inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy) 1299 { 1300 int result; 1301 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 1302 defined CONFIG_MPU_SENSORS_MPU6050B1 1303 unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, 1304 DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, 1305 DINAA0 + 3 1306 }; 1307 1308 if (inv_get_state() < INV_STATE_DMP_OPENED) 1309 return INV_ERROR_SM_IMPROPER_STATE; 1310 1311 if (accuracy) 1312 accuracy = INV_16_BIT; 1313 1314 elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); 1315 1316 if (elements & 1) 1317 fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; 1318 else 1319 fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; 1320 if (elements & 0x7e) 1321 fifo_obj.data_config[CONFIG_RAW_DATA] = 1322 (0x3f & (elements >> 1)) | INV_16_BIT; 1323 else 1324 fifo_obj.data_config[CONFIG_RAW_DATA] = 0; 1325 1326 if (elements & INV_ELEMENT_1) { 1327 regs[0] = DINACA; 1328 } 1329 if (elements & INV_ELEMENT_2) { 1330 regs[1] = DINBC4; 1331 } 1332 if (elements & INV_ELEMENT_3) { 1333 regs[2] = DINACC; 1334 } 1335 if (elements & INV_ELEMENT_4) { 1336 regs[3] = DINBC6; 1337 } 1338 if (elements & INV_ELEMENT_5) { 1339 regs[4] = DINBC0; 1340 } 1341 if (elements & INV_ELEMENT_6) { 1342 regs[5] = DINAC8; 1343 } 1344 if (elements & INV_ELEMENT_7) { 1345 regs[6] = DINBC2; 1346 } 1347 result = inv_set_mpu_memory(KEY_CFG_15, 7, regs); 1348 if (result) { 1349 LOG_RESULT_LOCATION(result); 1350 return result; 1351 } 1352 1353 return inv_set_footer(); 1354 1355 #else 1356 INVENSENSE_FUNC_START; 1357 unsigned char regs[4] = { DINAA0 + 3, 1358 DINAA0 + 3, 1359 DINAA0 + 3, 1360 DINAA0 + 3 1361 }; 1362 1363 if (inv_get_state() < INV_STATE_DMP_OPENED) 1364 return INV_ERROR_SM_IMPROPER_STATE; 1365 1366 if (accuracy) 1367 accuracy = INV_16_BIT; 1368 1369 elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); 1370 1371 if (elements & 0x03) { 1372 elements |= 0x03; 1373 regs[0] = DINA20; 1374 } 1375 if (elements & 0x0C) { 1376 elements |= 0x0C; 1377 regs[1] = DINA28; 1378 } 1379 if (elements & 0x30) { 1380 elements |= 0x30; 1381 regs[2] = DINA30; 1382 } 1383 if (elements & 0x40) { 1384 elements |= 0xC0; 1385 regs[3] = DINA38; 1386 } 1387 1388 result = inv_set_mpu_memory(KEY_CFG_15, 4, regs); 1389 if (result) { 1390 LOG_RESULT_LOCATION(result); 1391 return result; 1392 } 1393 1394 if (elements & 0x01) 1395 fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; 1396 else 1397 fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; 1398 if (elements & 0xfe) 1399 fifo_obj.data_config[CONFIG_RAW_DATA] = 1400 (0x7f & (elements >> 1)) | INV_16_BIT; 1401 else 1402 fifo_obj.data_config[CONFIG_RAW_DATA] = 0; 1403 1404 return inv_set_footer(); 1405 #endif 1406 } 1407 1408 /** Sends raw external data to the FIFO. 1409 * Should be called once after inv_dmp_open() and before inv_dmp_start(). 1410 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them 1411 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1412 * for a subset. 1413 * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data. 1414 * Sending and Stop sending are reference counted, so data actually 1415 * stops when the reference reaches zero. 1416 */ 1417 inv_error_t inv_send_external_sensor_data(uint_fast16_t elements, 1418 uint_fast16_t accuracy) 1419 { 1420 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 1421 defined CONFIG_MPU_SENSORS_MPU6050B1 1422 int result; 1423 unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3, 1424 DINAA0 + 3, DINAA0 + 3, 1425 DINAA0 + 3, DINAA0 + 3 }; 1426 1427 if (inv_get_state() < INV_STATE_DMP_OPENED) 1428 return INV_ERROR_SM_IMPROPER_STATE; 1429 1430 if (accuracy) 1431 accuracy = INV_16_BIT; 1432 1433 elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6); 1434 1435 if (elements) 1436 fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT; 1437 else 1438 fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0; 1439 1440 if (elements & INV_ELEMENT_1) { 1441 regs[0] = DINBC2; 1442 } 1443 if (elements & INV_ELEMENT_2) { 1444 regs[1] = DINACA; 1445 } 1446 if (elements & INV_ELEMENT_3) { 1447 regs[2] = DINBC4; 1448 } 1449 if (elements & INV_ELEMENT_4) { 1450 regs[3] = DINBC0; 1451 } 1452 if (elements & INV_ELEMENT_5) { 1453 regs[4] = DINAC8; 1454 } 1455 if (elements & INV_ELEMENT_6) { 1456 regs[5] = DINACC; 1457 } 1458 1459 result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs); 1460 if (result) { 1461 LOG_RESULT_LOCATION(result); 1462 return result; 1463 } 1464 1465 return inv_set_footer(); 1466 1467 #else 1468 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported 1469 #endif 1470 } 1471 1472 /** 1473 * @brief Send the Quantized Acceleromter data into the FIFO. The data can be 1474 * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel(). 1475 * 1476 * To be useful this should be set to fifo_rate + 1 if less than 1477 * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work. 1478 * 1479 * @param elements 1480 * the components bitmask. 1481 * To send all compoents use INV_ALL. 1482 * 1483 * @param accuracy 1484 * Use INV_32_BIT for 32-bit data or INV_16_BIT for 1485 * 16-bit data. 1486 * Set to zero to remove it from the FIFO. 1487 * 1488 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1489 */ 1490 inv_error_t inv_send_quantized_accel(uint_fast16_t elements, 1491 uint_fast16_t accuracy) 1492 { 1493 INVENSENSE_FUNC_START; 1494 unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, 1495 DINA30, DINA38 1496 }; 1497 unsigned char regs2[4] = { DINA20, DINA28, 1498 DINA30, DINA38 1499 }; 1500 inv_error_t result; 1501 int_fast8_t kk; 1502 int_fast8_t ii; 1503 1504 if (inv_get_state() < INV_STATE_DMP_OPENED) 1505 return INV_ERROR_SM_IMPROPER_STATE; 1506 1507 elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8); 1508 1509 if (elements) { 1510 fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT; 1511 } else { 1512 fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0; 1513 } 1514 1515 for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) { 1516 fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0; 1517 for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { 1518 fifo_obj.decoded_accel[kk][ii] = 0; 1519 } 1520 } 1521 1522 for (kk = 0; kk < 4; ++kk) { 1523 if ((elements & 1) == 0) 1524 regs[kk + 1] = DINAA0 + 3; 1525 elements >>= 1; 1526 } 1527 1528 result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs); 1529 if (result) { 1530 LOG_RESULT_LOCATION(result); 1531 return result; 1532 } 1533 1534 for (kk = 0; kk < 4; ++kk) { 1535 if ((elements & 1) == 0) 1536 regs2[kk] = DINAA0 + 3; 1537 elements >>= 1; 1538 } 1539 1540 result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2); 1541 if (result) { 1542 LOG_RESULT_LOCATION(result); 1543 return result; 1544 } 1545 1546 return inv_set_footer(); 1547 } 1548 1549 inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy) 1550 { 1551 INVENSENSE_FUNC_START; 1552 int_fast8_t kk; 1553 unsigned char regs[3] = { DINA28, DINA30, DINA38 }; 1554 inv_error_t result; 1555 1556 if (inv_get_state() < INV_STATE_DMP_OPENED) 1557 return INV_ERROR_SM_IMPROPER_STATE; 1558 1559 if (accuracy) { 1560 accuracy = INV_32_BIT; 1561 } 1562 1563 elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3); 1564 accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS); 1565 1566 fifo_obj.data_config[CONFIG_EIS] = elements | accuracy; 1567 1568 for (kk = 0; kk < 3; ++kk) { 1569 if ((elements & 1) == 0) 1570 regs[kk] = DINAA0 + 7; 1571 elements >>= 1; 1572 } 1573 1574 result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs); 1575 1576 return inv_set_footer(); 1577 } 1578 1579 /** 1580 * @brief Returns 3-element vector of accelerometer data in body frame. 1581 * 1582 * @param[out] data 3-element vector of accelerometer data in body frame. 1583 * One gee = 2^16. 1584 * @return 0 on success or an error code. 1585 */ 1586 inv_error_t inv_get_accel(long *data) 1587 { 1588 int kk; 1589 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1590 1591 if (data == NULL) 1592 return INV_ERROR_INVALID_PARAMETER; 1593 1594 if ((!fifo_obj.data_config[CONFIG_ACCEL] && 1595 (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) 1596 || 1597 (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && 1598 !inv_accel_present())) 1599 return INV_ERROR_FEATURE_NOT_ENABLED; 1600 1601 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1602 data[kk] = fifo_obj.decoded[REF_ACCEL + kk]; 1603 } 1604 1605 return INV_SUCCESS; 1606 } 1607 1608 /** 1609 * @brief Returns 4-element quaternion vector derived from 6-axis or 1610 * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is 1611 * gyros, accel and compass. 1612 * 1613 * @param[out] data 4-element quaternion vector. One is scaled to 2^30. 1614 * @return 0 on success or an error code. 1615 */ 1616 inv_error_t inv_get_quaternion(long *data) 1617 { 1618 int kk; 1619 1620 if (data == NULL) 1621 return INV_ERROR_INVALID_PARAMETER; 1622 1623 if (!fifo_obj.data_config[CONFIG_QUAT]) 1624 return INV_ERROR_FEATURE_NOT_ENABLED; 1625 1626 for (kk = 0; kk < 4; ++kk) { 1627 data[kk] = fifo_obj.decoded[REF_QUATERNION + kk]; 1628 } 1629 1630 return INV_SUCCESS; 1631 } 1632 1633 /** 1634 * @brief Returns 4-element quaternion vector derived from 6 1635 * axis sensors (gyros and accels). 1636 * @param[out] data 1637 * 4-element quaternion vector. One is scaled to 2^30. 1638 * @return 0 on success or an error code. 1639 */ 1640 inv_error_t inv_get_6axis_quaternion(long *data) 1641 { 1642 int kk; 1643 if (data == NULL) 1644 return INV_ERROR_INVALID_PARAMETER; 1645 1646 if (!fifo_obj.data_config[CONFIG_QUAT]) 1647 return INV_ERROR_FEATURE_NOT_ENABLED; 1648 1649 for (kk = 0; kk < 4; ++kk) { 1650 data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk]; 1651 } 1652 1653 return INV_SUCCESS; 1654 } 1655 1656 inv_error_t inv_get_relative_quaternion(long *data) 1657 { 1658 if (data == NULL) 1659 return INV_ERROR; 1660 data[0] = inv_obj.relative_quat[0]; 1661 data[1] = inv_obj.relative_quat[1]; 1662 data[2] = inv_obj.relative_quat[2]; 1663 data[3] = inv_obj.relative_quat[3]; 1664 return INV_SUCCESS; 1665 } 1666 1667 /** 1668 * @brief Returns 3-element vector of gyro data in body frame. 1669 * @param[out] data 1670 * 3-element vector of gyro data in body frame 1671 * with gravity removed. One degree per second = 2^16. 1672 * @return 0 on success or an error code. 1673 */ 1674 inv_error_t inv_get_gyro(long *data) 1675 { 1676 int kk; 1677 if (data == NULL) 1678 return INV_ERROR_INVALID_PARAMETER; 1679 1680 if (fifo_obj.data_config[CONFIG_GYROS]) { 1681 for (kk = 0; kk < 3; ++kk) { 1682 data[kk] = fifo_obj.decoded[REF_GYROS + kk]; 1683 } 1684 return INV_SUCCESS; 1685 } else { 1686 return INV_ERROR_FEATURE_NOT_ENABLED; 1687 } 1688 } 1689 1690 /** 1691 * @brief Get the 3-element gravity vector from the FIFO expressed 1692 * in coordinates relative to the body frame. 1693 * @param data 1694 * 3-element vector of gravity in body frame. 1695 * @return 0 on success or an error code. 1696 */ 1697 inv_error_t inv_get_gravity(long *data) 1698 { 1699 long quat[4]; 1700 int ii; 1701 inv_error_t result; 1702 1703 if (data == NULL) 1704 return INV_ERROR_INVALID_PARAMETER; 1705 1706 if (!fifo_obj.data_config[CONFIG_QUAT]) 1707 return INV_ERROR_FEATURE_NOT_ENABLED; 1708 1709 if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) { 1710 fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY; 1711 1712 // Compute it from Quaternion 1713 result = inv_get_quaternion(quat); 1714 if (result) { 1715 LOG_RESULT_LOCATION(result); 1716 return result; 1717 } 1718 1719 data[0] = 1720 inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); 1721 data[1] = 1722 inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); 1723 data[2] = 1724 (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) - 1725 1073741824L; 1726 1727 for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { 1728 data[ii] >>= 14; 1729 fifo_obj.gravity_cache[ii] = data[ii]; 1730 } 1731 } else { 1732 data[0] = fifo_obj.gravity_cache[0]; 1733 data[1] = fifo_obj.gravity_cache[1]; 1734 data[2] = fifo_obj.gravity_cache[2]; 1735 } 1736 1737 return INV_SUCCESS; 1738 } 1739 1740 /** 1741 * @brief Sets the filter coefficent used for computing the acceleration 1742 * bias which is used to compute linear acceleration. 1743 * @param[in] coef Fitler coefficient. 0. means no filter, a small number means 1744 * a small cutoff frequency with an increasing number meaning 1745 * an increasing cutoff frequency. 1746 */ 1747 inv_error_t inv_set_linear_accel_filter_coef(float coef) 1748 { 1749 fifo_obj.acc_filter_coef = coef; 1750 return INV_SUCCESS; 1751 } 1752 1753 /** 1754 * @brief Returns 3-element vector of accelerometer data in body frame 1755 * with gravity removed. 1756 * @param[out] data 3-element vector of accelerometer data in body frame 1757 * with gravity removed. One g = 2^16. 1758 * @return 0 on success or an error code. data unchanged on error. 1759 */ 1760 inv_error_t inv_get_linear_accel(long *data) 1761 { 1762 int kk; 1763 long grav[3]; 1764 long la[3]; 1765 inv_error_t result; 1766 1767 if (data == NULL) 1768 return INV_ERROR_INVALID_PARAMETER; 1769 1770 result = inv_get_gravity(grav); 1771 if (result) { 1772 LOG_RESULT_LOCATION(result); 1773 return result; 1774 } 1775 result = inv_get_accel(la); 1776 if (result) { 1777 LOG_RESULT_LOCATION(result); 1778 return result; 1779 } 1780 1781 if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) { 1782 fifo_obj.cache |= FIFO_CACHE_ACC_BIAS; 1783 1784 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1785 long x; 1786 x = la[kk] - grav[kk]; 1787 fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef + 1788 fifo_obj.acc_bias_filt[kk] * 1789 (1.f - 1790 fifo_obj.acc_filter_coef)); 1791 data[kk] = x - fifo_obj.acc_bias_filt[kk]; 1792 } 1793 } else { 1794 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1795 data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk]; 1796 } 1797 } 1798 return INV_SUCCESS; 1799 } 1800 1801 /** 1802 * @brief Returns 3-element vector of accelerometer data in world frame 1803 * with gravity removed. 1804 * @param[out] data 3-element vector of accelerometer data in world frame 1805 * with gravity removed. One g = 2^16. 1806 * @return 0 on success or an error code. 1807 */ 1808 inv_error_t inv_get_linear_accel_in_world(long *data) 1809 { 1810 int kk; 1811 if (data == NULL) 1812 return INV_ERROR_INVALID_PARAMETER; 1813 if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) { 1814 long wtemp[4], qi[4], wtemp2[4]; 1815 wtemp[0] = 0; 1816 inv_get_linear_accel(&wtemp[1]); 1817 inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2); 1818 inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi); 1819 inv_q_mult(wtemp2, qi, wtemp); 1820 for (kk = 0; kk < 3; ++kk) { 1821 data[kk] = wtemp[kk + 1]; 1822 } 1823 return INV_SUCCESS; 1824 } else { 1825 return INV_ERROR_FEATURE_NOT_ENABLED; 1826 } 1827 } 1828 1829 /** 1830 * @brief Returns 4-element vector of control data. 1831 * @param[out] data 4-element vector of control data. 1832 * @return 0 for succes or an error code. 1833 */ 1834 inv_error_t inv_get_cntrl_data(long *data) 1835 { 1836 int kk; 1837 if (data == NULL) 1838 return INV_ERROR_INVALID_PARAMETER; 1839 1840 if (!fifo_obj.data_config[CONFIG_CONTROL_DATA]) 1841 return INV_ERROR_FEATURE_NOT_ENABLED; 1842 1843 for (kk = 0; kk < 4; ++kk) { 1844 data[kk] = fifo_obj.decoded[REF_CONTROL + kk]; 1845 } 1846 1847 return INV_SUCCESS; 1848 1849 } 1850 1851 /** 1852 * @brief Returns 3-element vector of EIS shfit data 1853 * @param[out] data 3-element vector of EIS shift data. 1854 * @return 0 for succes or an error code. 1855 */ 1856 inv_error_t inv_get_eis(long *data) 1857 { 1858 int kk; 1859 if (data == NULL) 1860 return INV_ERROR_INVALID_PARAMETER; 1861 1862 if (!fifo_obj.data_config[CONFIG_EIS]) 1863 return INV_ERROR_FEATURE_NOT_ENABLED; 1864 1865 for (kk = 0; kk < 3; ++kk) { 1866 data[kk] = fifo_obj.decoded[REF_EIS + kk]; 1867 } 1868 1869 return INV_SUCCESS; 1870 1871 } 1872 1873 /** 1874 * @brief Returns 3-element vector of accelerometer data in body frame. 1875 * @param[out] data 3-element vector of accelerometer data in body frame in g's. 1876 * @return 0 for success or an error code. 1877 */ 1878 inv_error_t inv_get_accel_float(float *data) 1879 { 1880 long lData[3]; 1881 int kk; 1882 int result; 1883 1884 if (data == NULL) 1885 return INV_ERROR_INVALID_PARAMETER; 1886 1887 result = inv_get_accel(lData); 1888 if (result) { 1889 LOG_RESULT_LOCATION(result); 1890 return result; 1891 } 1892 1893 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1894 data[kk] = lData[kk] / 65536.0f; 1895 } 1896 1897 return INV_SUCCESS; 1898 } 1899 1900 /** 1901 * @brief Returns 4-element quaternion vector. 1902 * @param[out] data 4-element quaternion vector. 1903 * @return 0 on success, an error code otherwise. 1904 */ 1905 inv_error_t inv_get_quaternion_float(float *data) 1906 { 1907 int kk; 1908 1909 if (data == NULL) 1910 return INV_ERROR_INVALID_PARAMETER; 1911 1912 if (!fifo_obj.data_config[CONFIG_QUAT]) 1913 return INV_ERROR_FEATURE_NOT_ENABLED; 1914 1915 for (kk = 0; kk < 4; ++kk) { 1916 data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f; 1917 } 1918 1919 return INV_SUCCESS; 1920 } 1921 1922 /** 1923 * @brief Command the MPU to put data in the FIFO at a particular rate. 1924 * 1925 * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For 1926 * example if the MPU is running at 200Hz the following values apply: 1927 * 1928 * <TABLE> 1929 * <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR> 1930 * <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR> 1931 * <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR> 1932 * <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR> 1933 * <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR> 1934 * <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR> 1935 * <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR> 1936 * </TABLE> 1937 * 1938 * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED) 1939 * then inv_run_state_callbacks() will be called to allow features 1940 * that depend upon fundamental constants to be updated. 1941 * 1942 * @pre inv_dmp_open() 1943 * @ifnot MPL_MF 1944 * or inv_open_low_power_pedometer() 1945 * or inv_eis_open_dmp() 1946 * @endif 1947 * and inv_dmp_start() 1948 * must <b>NOT</b> have been called. 1949 * 1950 * @param fifoRate Divider value - 1. Output rate is 1951 * (DMP Sample Rate) / (fifoRate + 1). 1952 * 1953 * @return INV_SUCCESS if successful, ML error code on any failure. 1954 */ 1955 inv_error_t inv_set_fifo_rate(unsigned short fifoRate) 1956 { 1957 INVENSENSE_FUNC_START; 1958 unsigned char regs[2]; 1959 unsigned char state; 1960 inv_error_t result = INV_SUCCESS; 1961 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1962 1963 state = inv_get_state(); 1964 if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED) 1965 return INV_ERROR_SM_IMPROPER_STATE; 1966 1967 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { 1968 1969 fifo_obj.fifo_rate = fifoRate; 1970 1971 regs[0] = (unsigned char)((fifoRate >> 8) & 0xff); 1972 regs[1] = (unsigned char)(fifoRate & 0xff); 1973 1974 result = inv_set_mpu_memory(KEY_D_0_22, 2, regs); 1975 if (result) { 1976 LOG_RESULT_LOCATION(result); 1977 return result; 1978 } 1979 fifo_obj.sample_step_size_ms = 1980 (unsigned short)(((long)fifoRate + 1) * 1981 (inv_mpu_get_sampling_period_us 1982 (mldl_cfg)) / 1000L); 1983 1984 if (state == INV_STATE_DMP_STARTED) 1985 result = inv_run_state_callbacks(state); 1986 } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) { 1987 struct ext_slave_config config; 1988 long data; 1989 config.key = MPU_SLAVE_CONFIG_ODR_RESUME; 1990 config.len = sizeof(long); 1991 config.apply = (state == INV_STATE_DMP_STARTED); 1992 config.data = &data; 1993 data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1); 1994 1995 /* Ask for the same frequency */ 1996 result = inv_mpu_config_accel(mldl_cfg, 1997 inv_get_serial_handle(), 1998 inv_get_serial_handle(), &config); 1999 if (result) { 2000 LOG_RESULT_LOCATION(result); 2001 return result; 2002 } 2003 result = inv_mpu_get_accel_config(mldl_cfg, 2004 inv_get_serial_handle(), 2005 inv_get_serial_handle(), &config); 2006 if (result) { 2007 LOG_RESULT_LOCATION(result); 2008 return result; 2009 } 2010 if(FIFO_DEBUG) 2011 MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000); 2012 /* Record the actual frequency granted odr is in mHz */ 2013 fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data); 2014 } 2015 return result; 2016 } 2017 2018 /** 2019 * @brief Retrieve the current FIFO update divider - 1. 2020 * See inv_set_fifo_rate() for how this value is used. 2021 * 2022 * The fifo rate when there is no fifo is the equivilent divider when 2023 * derived from the value set by SetSampleSteSizeMs() 2024 * 2025 * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error. 2026 */ 2027 unsigned short inv_get_fifo_rate(void) 2028 { 2029 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 2030 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) 2031 return fifo_obj.fifo_rate; 2032 else 2033 return (unsigned short)((1000 * fifo_obj.sample_step_size_ms) / 2034 (inv_mpu_get_sampling_period_us(mldl_cfg))) - 1; 2035 } 2036 2037 /** 2038 * @brief Returns the step size for quaternion type data. 2039 * 2040 * Typically the data rate for each FIFO packet. When the gryos are sleeping 2041 * this value will return the last value set by SetSampleStepSizeMs() 2042 * 2043 * @return step size for quaternion type data 2044 */ 2045 int_fast16_t inv_get_sample_step_size_ms(void) 2046 { 2047 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 2048 2049 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) 2050 return (fifo_obj.fifo_rate + 1) * 2051 (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000); 2052 else 2053 return fifo_obj.sample_step_size_ms; 2054 } 2055 2056 /** 2057 * @brief Returns the step size for quaternion type data. 2058 * 2059 * Typically the data rate for each FIFO packet. When the gryos are sleeping 2060 * this value will return the last value set by SetSampleStepSizeMs() 2061 * 2062 * @return step size for quaternion type data 2063 */ 2064 int_fast16_t inv_get_sample_frequency(void) 2065 { 2066 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 2067 2068 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) 2069 return (inv_mpu_get_sampling_rate_hz(mldl_cfg) / 2070 (fifo_obj.fifo_rate + 1)); 2071 else 2072 return (1000 / fifo_obj.sample_step_size_ms); 2073 } 2074 2075 /** 2076 * @brief The gyro data magnitude squared : 2077 * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. 2078 * @return the computed magnitude squared output of the gyroscope. 2079 */ 2080 unsigned long inv_get_gyro_sum_of_sqr(void) 2081 { 2082 unsigned long gmag = 0; 2083 long temp; 2084 int kk; 2085 2086 for (kk = 0; kk < 3; ++kk) { 2087 temp = fifo_obj.decoded[REF_GYROS + kk] >> 2088 (16 - (GYRO_MAG_SQR_SHIFT / 2)); 2089 gmag += temp * temp; 2090 } 2091 2092 return gmag; 2093 } 2094 2095 /** 2096 * @brief The gyro data magnitude squared: 2097 * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT. 2098 * @return the computed magnitude squared output of the accelerometer. 2099 */ 2100 unsigned long inv_accel_sum_of_sqr(void) 2101 { 2102 unsigned long amag = 0; 2103 long temp; 2104 int kk; 2105 long accel[3]; 2106 inv_error_t result; 2107 2108 result = inv_get_accel(accel); 2109 if (INV_SUCCESS != result) { 2110 return 0; 2111 } 2112 2113 for (kk = 0; kk < 3; ++kk) { 2114 temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2)); 2115 amag += temp * temp; 2116 } 2117 return amag; 2118 } 2119 2120 /** 2121 * @internal 2122 */ 2123 void inv_override_quaternion(float *q) 2124 { 2125 int kk; 2126 for (kk = 0; kk < 4; ++kk) { 2127 fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30)); 2128 } 2129 } 2130 2131 /** 2132 * @internal 2133 * @brief This registers a function to be called for each set of 2134 * gyro/quaternion/rotation matrix/etc output. 2135 * @param[in] func The callback function to register 2136 * @param[in] priority The unique priority number of the callback. Lower numbers 2137 * are called first. 2138 * @return error code. 2139 */ 2140 inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority) 2141 { 2142 INVENSENSE_FUNC_START; 2143 inv_error_t result; 2144 int kk, nn; 2145 2146 result = inv_lock_mutex(fifo_rate_obj.mutex); 2147 if (INV_SUCCESS != result) { 2148 return result; 2149 } 2150 // Make sure we haven't registered this function already 2151 // Or used the same priority 2152 for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { 2153 if ((fifo_rate_obj.fifo_process_cb[kk] == func) || 2154 (fifo_rate_obj.priority[kk] == priority)) { 2155 inv_unlock_mutex(fifo_rate_obj.mutex); 2156 return INV_ERROR_INVALID_PARAMETER; 2157 } 2158 } 2159 2160 // Make sure we have not filled up our number of allowable callbacks 2161 if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) { 2162 kk = 0; 2163 if (fifo_rate_obj.num_cb != 0) { 2164 // set kk to be where this new callback goes in the array 2165 while ((kk < fifo_rate_obj.num_cb) && 2166 (fifo_rate_obj.priority[kk] < priority)) { 2167 kk++; 2168 } 2169 if (kk != fifo_rate_obj.num_cb) { 2170 // We need to move the others 2171 for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) { 2172 fifo_rate_obj.fifo_process_cb[nn] = 2173 fifo_rate_obj.fifo_process_cb[nn - 1]; 2174 fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1]; 2175 } 2176 } 2177 } 2178 // Add new callback 2179 fifo_rate_obj.fifo_process_cb[kk] = func; 2180 fifo_rate_obj.priority[kk] = priority; 2181 fifo_rate_obj.num_cb++; 2182 } else { 2183 result = INV_ERROR_MEMORY_EXAUSTED; 2184 } 2185 2186 inv_unlock_mutex(fifo_rate_obj.mutex); 2187 return result; 2188 } 2189 2190 /** 2191 * @internal 2192 * @brief This unregisters a function to be called for each set of 2193 * gyro/quaternion/rotation matrix/etc output. 2194 * @return error code. 2195 */ 2196 inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func) 2197 { 2198 INVENSENSE_FUNC_START; 2199 int kk, jj; 2200 inv_error_t result; 2201 2202 result = inv_lock_mutex(fifo_rate_obj.mutex); 2203 if (INV_SUCCESS != result) { 2204 return result; 2205 } 2206 // Make sure we haven't registered this function already 2207 result = INV_ERROR_INVALID_PARAMETER; 2208 for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { 2209 if (fifo_rate_obj.fifo_process_cb[kk] == func) { 2210 for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) { 2211 fifo_rate_obj.fifo_process_cb[jj - 1] = 2212 fifo_rate_obj.fifo_process_cb[jj]; 2213 fifo_rate_obj.priority[jj - 1] = 2214 fifo_rate_obj.priority[jj]; 2215 } 2216 fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL; 2217 fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0; 2218 fifo_rate_obj.num_cb--; 2219 result = INV_SUCCESS; 2220 break; 2221 } 2222 } 2223 2224 inv_unlock_mutex(fifo_rate_obj.mutex); 2225 return result; 2226 2227 } 2228 #ifdef UMPL 2229 bool bFIFIDataAvailable = FALSE; 2230 bool isUmplDataInFIFO(void) 2231 { 2232 return bFIFIDataAvailable; 2233 } 2234 void setUmplDataInFIFOFlag(bool flag) 2235 { 2236 bFIFIDataAvailable = flag; 2237 } 2238 #endif 2239 inv_error_t inv_run_fifo_rate_processes(void) 2240 { 2241 int kk; 2242 inv_error_t result, result2; 2243 2244 result = inv_lock_mutex(fifo_rate_obj.mutex); 2245 if (INV_SUCCESS != result) { 2246 MPL_LOGE("MLOsLockMutex returned %d\n", result); 2247 return result; 2248 } 2249 // User callbacks take priority over the fifo_process_cb callback 2250 if (fifo_obj.fifo_process_cb) 2251 fifo_obj.fifo_process_cb(); 2252 2253 for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { 2254 if (fifo_rate_obj.fifo_process_cb[kk]) { 2255 result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj); 2256 if (result == INV_SUCCESS) 2257 #ifdef UMPL 2258 setUmplDataInFIFOFlag(TRUE); 2259 #endif 2260 result = result2; 2261 } 2262 } 2263 2264 inv_unlock_mutex(fifo_rate_obj.mutex); 2265 return result; 2266 } 2267 2268 /*********************/ 2269 /** \}*//* defgroup */ 2270 /*********************/ 2271
