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: mldl.c 5653 2011-06-16 21:06:55Z nroyer $ 21 * 22 *****************************************************************************/ 23 24 /** 25 * @defgroup MLDL 26 * @brief Motion Library - Driver Layer. 27 * The Motion Library Driver Layer provides the intrface to the 28 * system drivers that are used by the Motion Library. 29 * 30 * @{ 31 * @file mldl.c 32 * @brief The Motion Library Driver Layer. 33 */ 34 35 /* ------------------ */ 36 /* - Include Files. - */ 37 /* ------------------ */ 38 39 #include <string.h> 40 41 #include "mpu.h" 42 #if defined CONFIG_MPU_SENSORS_MPU6050A2 43 # include "mpu6050a2.h" 44 #elif defined CONFIG_MPU_SENSORS_MPU6050B1 45 # include "mpu6050b1.h" 46 #elif defined CONFIG_MPU_SENSORS_MPU3050 47 # include "mpu3050.h" 48 #else 49 #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx 50 #endif 51 #include "mldl.h" 52 #include "mldl_cfg.h" 53 #include "compass.h" 54 #include "mlsl.h" 55 #include "mlos.h" 56 #include "mlinclude.h" 57 #include "ml.h" 58 #include "dmpKey.h" 59 #include "mlFIFOHW.h" 60 #include "compass.h" 61 #include "pressure.h" 62 63 #include "log.h" 64 #undef MPL_LOG_TAG 65 #define MPL_LOG_TAG "MPL-mldl" 66 67 #define _mldlDebug(x) //{x} 68 69 /* --------------------- */ 70 /* - Variables. - */ 71 /* --------------------- */ 72 73 #define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */ 74 75 /*---- structure containing control variables used by MLDL ----*/ 76 static struct mldl_cfg mldlCfg; 77 struct ext_slave_descr gAccel; 78 struct ext_slave_descr gCompass; 79 struct ext_slave_descr gPressure; 80 struct mpu_platform_data gPdata; 81 static void *sMLSLHandle; 82 int_fast8_t intTrigger[NUM_OF_INTSOURCES]; 83 84 /******************************************************************************* 85 * Functions for accessing the DMP memory via keys 86 ******************************************************************************/ 87 88 unsigned short (*sGetAddress) (unsigned short key) = NULL; 89 static const unsigned char *localDmpMemory = NULL; 90 static unsigned short localDmpMemorySize = 0; 91 92 /** 93 * @internal 94 * @brief Sets the function to use to convert keys to addresses. This 95 * will changed for each DMP code loaded. 96 * @param func 97 * Function used to convert keys to addresses. 98 * @endif 99 */ 100 void inv_set_get_address(unsigned short (*func) (unsigned short key)) 101 { 102 INVENSENSE_FUNC_START; 103 _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func); 104 ) 105 sGetAddress = func; 106 } 107 108 /** 109 * @internal 110 * @brief Check if the feature is supported in the currently loaded 111 * DMP code basing on the fact that the key is assigned a 112 * value or not. 113 * @param key the DMP key 114 * @return whether the feature associated with the key is supported 115 * or not. 116 */ 117 uint_fast8_t inv_dmpkey_supported(unsigned short key) 118 { 119 unsigned short memAddr; 120 121 if (sGetAddress == NULL) { 122 MPL_LOGE("%s : sGetAddress is NULL\n", __func__); 123 return FALSE; 124 } 125 126 memAddr = sGetAddress(key); 127 if (memAddr >= 0xffff) { 128 MPL_LOGV("inv_set_mpu_memory unsupported key\n"); 129 return FALSE; 130 } 131 132 return TRUE; 133 } 134 135 /** 136 * @internal 137 * @brief used to get the specified number of bytes from the original 138 * MPU memory location specified by the key. 139 * Reads the specified number of bytes from the MPU location 140 * that was used to program the MPU specified by the key. Each 141 * set of code specifies a function that changes keys into 142 * addresses. This function is set with setGetAddress(). 143 * 144 * @param key The key to use when looking up the address. 145 * @param length Number of bytes to read. 146 * @param buffer Result for data. 147 * 148 * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key 149 * not corresponding to a memory address will result in INV_ERROR. 150 * @endif 151 */ 152 inv_error_t inv_get_mpu_memory_original(unsigned short key, 153 unsigned short length, 154 unsigned char *buffer) 155 { 156 unsigned short offset; 157 158 if (sGetAddress == NULL) { 159 return INV_ERROR_NOT_OPENED; 160 } 161 162 offset = sGetAddress(key); 163 if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) { 164 return INV_ERROR_INVALID_PARAMETER; 165 } 166 167 memcpy(buffer, &localDmpMemory[offset], length); 168 169 return INV_SUCCESS; 170 } 171 172 unsigned short inv_dl_get_address(unsigned short key) 173 { 174 unsigned short offset; 175 if (sGetAddress == NULL) { 176 return INV_ERROR_NOT_OPENED; 177 } 178 179 offset = sGetAddress(key); 180 return offset; 181 } 182 183 /* ---------------------- */ 184 /* - Static Functions. - */ 185 /* ---------------------- */ 186 187 /** 188 * @brief Open the driver layer and resets the internal 189 * gyroscope, accelerometer, and compass data 190 * structures. 191 * @param mlslHandle 192 * the serial handle. 193 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 194 */ 195 inv_error_t inv_dl_open(void *mlslHandle) 196 { 197 inv_error_t result; 198 memset(&mldlCfg, 0, sizeof(mldlCfg)); 199 memset(intTrigger, INT_CLEAR, sizeof(intTrigger)); 200 201 sMLSLHandle = mlslHandle; 202 203 mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */ 204 mldlCfg.accel = &gAccel; 205 mldlCfg.compass = &gCompass; 206 mldlCfg.pressure = &gPressure; 207 mldlCfg.pdata = &gPdata; 208 209 result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle, 210 sMLSLHandle, sMLSLHandle, sMLSLHandle); 211 return result; 212 } 213 214 /** 215 * @brief Closes/Cleans up the ML Driver Layer. 216 * Put the device in sleep mode. 217 * @return INV_SUCCESS or non-zero error code. 218 */ 219 inv_error_t inv_dl_close(void) 220 { 221 INVENSENSE_FUNC_START; 222 inv_error_t result = INV_SUCCESS; 223 224 result = (inv_error_t) inv_mpu_suspend(&mldlCfg, 225 sMLSLHandle, 226 sMLSLHandle, 227 sMLSLHandle, 228 sMLSLHandle, 229 INV_ALL_SENSORS); 230 231 result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle, 232 sMLSLHandle, sMLSLHandle, sMLSLHandle); 233 /* Clear all previous settings */ 234 memset(&mldlCfg, 0, sizeof(mldlCfg)); 235 sMLSLHandle = NULL; 236 sGetAddress = NULL; 237 return result; 238 } 239 240 /** 241 * @brief Sets the requested_sensors 242 * 243 * Accessor to set the requested_sensors field of the mldl_cfg structure. 244 * Typically set at initialization. 245 * 246 * @param sensors 247 * Bitfield of the sensors that are going to be used. Combination of the 248 * following: 249 * - INV_X_GYRO 250 * - INV_Y_GYRO 251 * - INV_Z_GYRO 252 * - INV_DMP_PROCESSOR 253 * - INV_X_ACCEL 254 * - INV_Y_ACCEL 255 * - INV_Z_ACCEL 256 * - INV_X_COMPASS 257 * - INV_Y_COMPASS 258 * - INV_Z_COMPASS 259 * - INV_X_PRESSURE 260 * - INV_Y_PRESSURE 261 * - INV_Z_PRESSURE 262 * - INV_THREE_AXIS_GYRO 263 * - INV_THREE_AXIS_ACCEL 264 * - INV_THREE_AXIS_COMPASS 265 * - INV_THREE_AXIS_PRESSURE 266 * 267 * @return INV_SUCCESS or non-zero error code 268 */ 269 inv_error_t inv_init_requested_sensors(unsigned long sensors) 270 { 271 mldlCfg.requested_sensors = sensors; 272 273 return INV_SUCCESS; 274 } 275 276 /** 277 * @brief Starts the DMP running 278 * 279 * Resumes the sensor if any of the sensor axis or components are requested 280 * 281 * @param sensors 282 * Bitfield of the sensors to turn on. Combination of the following: 283 * - INV_X_GYRO 284 * - INV_Y_GYRO 285 * - INV_Z_GYRO 286 * - INV_DMP_PROCESSOR 287 * - INV_X_ACCEL 288 * - INV_Y_ACCEL 289 * - INV_Z_ACCEL 290 * - INV_X_COMPASS 291 * - INV_Y_COMPASS 292 * - INV_Z_COMPASS 293 * - INV_X_PRESSURE 294 * - INV_Y_PRESSURE 295 * - INV_Z_PRESSURE 296 * - INV_THREE_AXIS_GYRO 297 * - INV_THREE_AXIS_ACCEL 298 * - INV_THREE_AXIS_COMPASS 299 * - INV_THREE_AXIS_PRESSURE 300 * 301 * @return INV_SUCCESS or non-zero error code 302 */ 303 inv_error_t inv_dl_start(unsigned long sensors) 304 { 305 INVENSENSE_FUNC_START; 306 inv_error_t result = INV_SUCCESS; 307 308 mldlCfg.requested_sensors = sensors; 309 result = inv_mpu_resume(&mldlCfg, 310 sMLSLHandle, 311 sMLSLHandle, 312 sMLSLHandle, 313 sMLSLHandle, 314 sensors); 315 return result; 316 } 317 318 /** 319 * @brief Stops the DMP running and puts it in low power as requested 320 * 321 * Suspends each sensor according to the bitfield, if all axis and components 322 * of the sensor is off. 323 * 324 * @param sensors Bitfiled of the sensors to leave on. Combination of the 325 * following: 326 * - INV_X_GYRO 327 * - INV_Y_GYRO 328 * - INV_Z_GYRO 329 * - INV_X_ACCEL 330 * - INV_Y_ACCEL 331 * - INV_Z_ACCEL 332 * - INV_X_COMPASS 333 * - INV_Y_COMPASS 334 * - INV_Z_COMPASS 335 * - INV_X_PRESSURE 336 * - INV_Y_PRESSURE 337 * - INV_Z_PRESSURE 338 * - INV_THREE_AXIS_GYRO 339 * - INV_THREE_AXIS_ACCEL 340 * - INV_THREE_AXIS_COMPASS 341 * - INV_THREE_AXIS_PRESSURE 342 * 343 * 344 * @return INV_SUCCESS or non-zero error code 345 */ 346 inv_error_t inv_dl_stop(unsigned long sensors) 347 { 348 INVENSENSE_FUNC_START; 349 inv_error_t result = INV_SUCCESS; 350 351 result = inv_mpu_suspend(&mldlCfg, 352 sMLSLHandle, 353 sMLSLHandle, 354 sMLSLHandle, 355 sMLSLHandle, 356 sensors); 357 return result; 358 } 359 360 /** 361 * @brief Get a pointer to the internal data structure 362 * storing the configuration for the MPU, the accelerometer 363 * and the compass in use. 364 * @return a pointer to the data structure of type 'struct mldl_cfg'. 365 */ 366 struct mldl_cfg *inv_get_dl_config(void) 367 { 368 return &mldlCfg; 369 } 370 371 /** 372 * @brief Query the MPU slave address. 373 * @return The 7-bit mpu slave address. 374 */ 375 unsigned char inv_get_mpu_slave_addr(void) 376 { 377 INVENSENSE_FUNC_START; 378 return mldlCfg.addr; 379 } 380 381 /** 382 * @internal 383 * @brief MLDLCfgDMP configures the Digital Motion Processor internal to 384 * the MPU. The DMP can be enabled or disabled and the start address 385 * can be set. 386 * 387 * @param enableRun Enables the DMP processing if set to TRUE. 388 * @param enableFIFO Enables DMP output to the FIFO if set to TRUE. 389 * @param startAddress start address 390 * 391 * @return Zero if the command is successful, an error code otherwise. 392 */ 393 inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, 394 unsigned char enableFIFO) 395 { 396 INVENSENSE_FUNC_START; 397 398 mldlCfg.dmp_enable = enableRun; 399 mldlCfg.fifo_enable = enableFIFO; 400 mldlCfg.gyro_needs_reset = TRUE; 401 402 return INV_SUCCESS; 403 } 404 405 /** 406 * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin. 407 * The basic interrupt signal characteristics can be set 408 * (i.e. active high/low, open drain/push pull, etc.) and the 409 * triggers can be set. 410 * Currently only INTPIN_MPU is supported. 411 * 412 * @param triggers 413 * bitmask of triggers to enable for interrupt. 414 * The available triggers are: 415 * - BIT_MPU_RDY_EN 416 * - BIT_DMP_INT_EN 417 * - BIT_RAW_RDY_EN 418 * 419 * @return Zero if the command is successful, an error code otherwise. 420 */ 421 inv_error_t inv_get_dl_cfg_int(unsigned char triggers) 422 { 423 inv_error_t result = INV_SUCCESS; 424 425 #if defined CONFIG_MPU_SENSORS_MPU3050 426 /* Mantis has 8 bits of interrupt config bits */ 427 if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) { 428 return INV_ERROR_INVALID_PARAMETER; 429 } 430 #endif 431 432 mldlCfg.int_config = triggers; 433 if (!mldlCfg.gyro_is_suspended) { 434 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 435 MPUREG_INT_CFG, 436 (mldlCfg.int_config | mldlCfg.pdata-> 437 int_config)); 438 } else { 439 mldlCfg.gyro_needs_reset = TRUE; 440 } 441 442 return result; 443 } 444 445 /** 446 * @brief configures the output sampling rate on the MPU. 447 * Three parameters control the sampling: 448 * 449 * 1) Low pass filter bandwidth, and 450 * 2) output sampling divider. 451 * 452 * The output sampling rate is determined by the divider and the low 453 * pass filter setting. If the low pass filter is set to 454 * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the 455 * divider is 8kHz; for all other settings it is 1kHz. 456 * The 8-bit divider will divide this frequency to get the resulting 457 * sample frequency. 458 * For example, if the filter setting is not 256Hz and the divider is 459 * set to 7, then the sample rate is as follows: 460 * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms). 461 * 462 * The low pass filter selection codes control both the cutoff frequency of 463 * the internal low pass filter and internal analog sampling rate. The 464 * latter, in turn, affects the final output sampling rate according to the 465 * sample rate divider settig. 466 * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate, 467 * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate, 468 * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate, 469 * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate, 470 * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate, 471 * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate, 472 * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate, 473 * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate. 474 * 475 * @param lpf low pass filter, 0 to 7. 476 * @param divider Output sampling rate divider, 0 to 255. 477 * 478 * @return ML_SUCESS if successful; a non-zero error code otherwise. 479 **/ 480 inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider) 481 { 482 /*---- do range checking ----*/ 483 if (lpf >= NUM_MPU_FILTER) { 484 return INV_ERROR_INVALID_PARAMETER; 485 } 486 487 mldlCfg.lpf = lpf; 488 mldlCfg.divider = divider; 489 mldlCfg.gyro_needs_reset = TRUE; 490 491 return INV_SUCCESS; 492 } 493 494 /** 495 * @brief set the full scale range for the gyros. 496 * The full scale selection codes correspond to: 497 * 0 -> 250 dps, 498 * 1 -> 500 dps, 499 * 2 -> 1000 dps, 500 * 3 -> 2000 dps. 501 * Full scale range affect the MPU's measurement 502 * sensitivity. 503 * 504 * @param fullScale 505 * the gyro full scale range in dps. 506 * 507 * @return INV_SUCCESS or non-zero error code. 508 **/ 509 inv_error_t inv_set_full_scale(float fullScale) 510 { 511 if (fullScale == 250.f) 512 mldlCfg.full_scale = MPU_FS_250DPS; 513 else if (fullScale == 500.f) 514 mldlCfg.full_scale = MPU_FS_500DPS; 515 else if (fullScale == 1000.f) 516 mldlCfg.full_scale = MPU_FS_1000DPS; 517 else if (fullScale == 2000.f) 518 mldlCfg.full_scale = MPU_FS_2000DPS; 519 else { // not a valid setting 520 MPL_LOGE("Invalid full scale range specification for gyros : %f\n", 521 fullScale); 522 MPL_LOGE 523 ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n"); 524 return INV_ERROR_INVALID_PARAMETER; 525 } 526 mldlCfg.gyro_needs_reset = TRUE; 527 528 return INV_SUCCESS; 529 } 530 531 /** 532 * @brief This function sets the external sync for the MPU sampling. 533 * It can be synchronized on the LSB of any of the gyros, any of the 534 * external accels, or on the temp readings. 535 * 536 * @param extSync External sync selection, 0 to 7. 537 * @return Zero if the command is successful; an error code otherwise. 538 **/ 539 inv_error_t inv_set_external_sync(unsigned char extSync) 540 { 541 INVENSENSE_FUNC_START; 542 543 /*---- do range checking ----*/ 544 if (extSync >= NUM_MPU_EXT_SYNC) { 545 return INV_ERROR_INVALID_PARAMETER; 546 } 547 mldlCfg.ext_sync = extSync; 548 mldlCfg.gyro_needs_reset = TRUE; 549 550 return INV_SUCCESS; 551 } 552 553 inv_error_t inv_set_ignore_system_suspend(unsigned char ignore) 554 { 555 INVENSENSE_FUNC_START; 556 557 mldlCfg.ignore_system_suspend = ignore; 558 559 return INV_SUCCESS; 560 } 561 562 /** 563 * @brief inv_clock_source function sets the clock source for the MPU gyro 564 * processing. 565 * The source can be any of the following: 566 * - Internal 8MHz oscillator, 567 * - PLL with X gyro as reference, 568 * - PLL with Y gyro as reference, 569 * - PLL with Z gyro as reference, 570 * - PLL with external 32.768Mhz reference, or 571 * - PLL with external 19.2MHz reference 572 * 573 * For best accuracy and timing, it is highly recommended to use one 574 * of the gyros as the clock source; however this gyro must be 575 * enabled to use its clock (see 'MLDLPowerMgmtMPU()'). 576 * 577 * @param clkSource Clock source selection. 578 * Can be one of: 579 * - CLK_INTERNAL, 580 * - CLK_PLLGYROX, 581 * - CLK_PLLGYROY, 582 * - CLK_PLLGYROZ, 583 * - CLK_PLLEXT32K, or 584 * - CLK_PLLEXT19M. 585 * 586 * @return Zero if the command is successful; an error code otherwise. 587 **/ 588 inv_error_t inv_clock_source(unsigned char clkSource) 589 { 590 INVENSENSE_FUNC_START; 591 592 /*---- do range checking ----*/ 593 if (clkSource >= NUM_CLK_SEL) { 594 return INV_ERROR_INVALID_PARAMETER; 595 } 596 597 mldlCfg.clk_src = clkSource; 598 mldlCfg.gyro_needs_reset = TRUE; 599 600 return INV_SUCCESS; 601 } 602 603 /** 604 * @brief Set the Temperature Compensation offset. 605 * @param tc 606 * a pointer to the temperature compensations offset 607 * for the 3 gyro axes. 608 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 609 */ 610 inv_error_t inv_set_offsetTC(const unsigned char *tc) 611 { 612 int ii; 613 inv_error_t result; 614 615 for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) { 616 mldlCfg.offset_tc[ii] = tc[ii]; 617 } 618 619 if (!mldlCfg.gyro_is_suspended) { 620 #ifdef CONFIG_MPU_SENSORS_MPU3050 621 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 622 MPUREG_XG_OFFS_TC, tc[0]); 623 if (result) { 624 LOG_RESULT_LOCATION(result); 625 return result; 626 } 627 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 628 MPUREG_YG_OFFS_TC, tc[1]); 629 if (result) { 630 LOG_RESULT_LOCATION(result); 631 return result; 632 } 633 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 634 MPUREG_ZG_OFFS_TC, tc[2]); 635 if (result) { 636 LOG_RESULT_LOCATION(result); 637 return result; 638 } 639 #else 640 unsigned char reg; 641 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 642 MPUREG_XG_OFFS_TC, 643 ((mldlCfg.offset_tc[0] << 1) 644 & BITS_XG_OFFS_TC)); 645 if (result) { 646 LOG_RESULT_LOCATION(result); 647 return result; 648 } 649 reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC); 650 #ifdef CONFIG_MPU_SENSORS_MPU6050B1 651 if (mldlCfg.pdata->level_shifter) 652 reg |= BIT_I2C_MST_VDDIO; 653 #endif 654 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 655 MPUREG_YG_OFFS_TC, reg); 656 if (result) { 657 LOG_RESULT_LOCATION(result); 658 return result; 659 } 660 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 661 MPUREG_ZG_OFFS_TC, 662 ((mldlCfg.offset_tc[2] << 1) 663 & BITS_ZG_OFFS_TC)); 664 if (result) { 665 LOG_RESULT_LOCATION(result); 666 return result; 667 } 668 #endif 669 } else { 670 mldlCfg.gyro_needs_reset = TRUE; 671 } 672 return INV_SUCCESS; 673 } 674 675 /** 676 * @brief Set the gyro offset. 677 * @param offset 678 * a pointer to the gyro offset for the 3 gyro axes. This is scaled 679 * as it would be written to the hardware registers. 680 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 681 */ 682 inv_error_t inv_set_offset(const short *offset) 683 { 684 inv_error_t result; 685 unsigned char regs[7]; 686 int ii; 687 long sf; 688 689 sf = (2000L * 131) / mldlCfg.gyro_sens_trim; 690 for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) { 691 // Record the bias in the units the register uses 692 mldlCfg.offset[ii] = offset[ii]; 693 // Convert the bias to 1 dps = 1<<16 694 inv_obj.gyro_bias[ii] = -offset[ii] * sf; 695 regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff; 696 regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff); 697 } 698 699 if (!mldlCfg.gyro_is_suspended) { 700 regs[0] = MPUREG_X_OFFS_USRH; 701 result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs); 702 if (result) { 703 LOG_RESULT_LOCATION(result); 704 return result; 705 } 706 } else { 707 mldlCfg.gyro_needs_reset = TRUE; 708 } 709 return INV_SUCCESS; 710 } 711 712 /** 713 * @internal 714 * @brief used to get the specified number of bytes in the specified MPU 715 * memory bank. 716 * The memory bank is one of the following: 717 * - MPUMEM_RAM_BANK_0, 718 * - MPUMEM_RAM_BANK_1, 719 * - MPUMEM_RAM_BANK_2, or 720 * - MPUMEM_RAM_BANK_3. 721 * 722 * @param bank Memory bank to write. 723 * @param memAddr Starting address for write. 724 * @param length Number of bytes to write. 725 * @param buffer Result for data. 726 * 727 * @return zero if the command is successful, an error code otherwise. 728 * @endif 729 */ 730 inv_error_t 731 inv_get_mpu_memory_one_bank(unsigned char bank, 732 unsigned char memAddr, 733 unsigned short length, unsigned char *buffer) 734 { 735 inv_error_t result; 736 737 if ((bank >= MPU_MEM_NUM_RAM_BANKS) || 738 //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255 739 ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { 740 return INV_ERROR_INVALID_PARAMETER; 741 } 742 743 if (mldlCfg.gyro_is_suspended) { 744 memcpy(buffer, &mldlCfg.ram[bank][memAddr], length); 745 result = INV_SUCCESS; 746 } else { 747 result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr, 748 ((bank << 8) | memAddr), length, buffer); 749 if (result) { 750 LOG_RESULT_LOCATION(result); 751 return result; 752 } 753 } 754 755 return result; 756 } 757 758 /** 759 * @internal 760 * @brief used to set the specified number of bytes in the specified MPU 761 * memory bank. 762 * The memory bank is one of the following: 763 * - MPUMEM_RAM_BANK_0, 764 * - MPUMEM_RAM_BANK_1, 765 * - MPUMEM_RAM_BANK_2, or 766 * - MPUMEM_RAM_BANK_3. 767 * 768 * @param bank Memory bank to write. 769 * @param memAddr Starting address for write. 770 * @param length Number of bytes to write. 771 * @param buffer Result for data. 772 * 773 * @return zero if the command is successful, an error code otherwise. 774 * @endif 775 */ 776 inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, 777 unsigned short memAddr, 778 unsigned short length, 779 const unsigned char *buffer) 780 { 781 inv_error_t result = INV_SUCCESS; 782 int different; 783 784 if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) || 785 ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { 786 return INV_ERROR_INVALID_PARAMETER; 787 } 788 789 different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length); 790 memcpy(&mldlCfg.ram[bank][memAddr], buffer, length); 791 if (!mldlCfg.gyro_is_suspended) { 792 result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr, 793 ((bank << 8) | memAddr), length, buffer); 794 if (result) { 795 LOG_RESULT_LOCATION(result); 796 return result; 797 } 798 } else if (different) { 799 mldlCfg.gyro_needs_reset = TRUE; 800 } 801 802 return result; 803 } 804 805 /** 806 * @internal 807 * @brief used to get the specified number of bytes from the MPU location 808 * specified by the key. 809 * Reads the specified number of bytes from the MPU location 810 * specified by the key. Each set of code specifies a function 811 * that changes keys into addresses. This function is set with 812 * setGetAddress(). 813 * 814 * @param key The key to use when looking up the address. 815 * @param length Number of bytes to read. 816 * @param buffer Result for data. 817 * 818 * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key 819 * not corresponding to a memory address will result in INV_ERROR. 820 * @endif 821 */ 822 inv_error_t inv_get_mpu_memory(unsigned short key, 823 unsigned short length, unsigned char *buffer) 824 { 825 unsigned char bank; 826 inv_error_t result; 827 unsigned short memAddr; 828 829 if (sGetAddress == NULL) { 830 return INV_ERROR_NOT_OPENED; 831 } 832 833 memAddr = sGetAddress(key); 834 if (memAddr >= 0xffff) 835 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 836 bank = memAddr >> 8; // Get Bank 837 memAddr &= 0xff; 838 839 while (memAddr + length > MPU_MEM_BANK_SIZE) { 840 // We cross a bank in the middle 841 unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; 842 result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, 843 sub_length, buffer); 844 if (INV_SUCCESS != result) 845 return result; 846 bank++; 847 length -= sub_length; 848 buffer += sub_length; 849 memAddr = 0; 850 } 851 result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, 852 length, buffer); 853 854 if (result) { 855 LOG_RESULT_LOCATION(result); 856 return result; 857 } 858 859 return result; 860 } 861 862 /** 863 * @internal 864 * @brief used to set the specified number of bytes from the MPU location 865 * specified by the key. 866 * Set the specified number of bytes from the MPU location 867 * specified by the key. Each set of DMP code specifies a function 868 * that changes keys into addresses. This function is set with 869 * setGetAddress(). 870 * 871 * @param key The key to use when looking up the address. 872 * @param length Number of bytes to write. 873 * @param buffer Result for data. 874 * 875 * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key 876 * not corresponding to a memory address will result in INV_ERROR. 877 * @endif 878 */ 879 inv_error_t inv_set_mpu_memory(unsigned short key, 880 unsigned short length, 881 const unsigned char *buffer) 882 { 883 inv_error_t result = INV_SUCCESS; 884 unsigned short memAddr; 885 unsigned char bank; 886 887 if (sGetAddress == NULL) { 888 MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n"); 889 return INV_ERROR_INVALID_MODULE; 890 } 891 memAddr = sGetAddress(key); 892 893 if (memAddr >= 0xffff) { 894 MPL_LOGE("inv_set_mpu_memory unsupported key\n"); 895 return INV_ERROR_INVALID_MODULE; // This key not supported 896 } 897 898 bank = (unsigned char)(memAddr >> 8); 899 memAddr &= 0xff; 900 901 while (memAddr + length > MPU_MEM_BANK_SIZE) { 902 // We cross a bank in the middle 903 unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; 904 905 result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer); 906 if (result) { 907 LOG_RESULT_LOCATION(result); 908 return result; 909 } 910 911 bank++; 912 length -= sub_length; 913 buffer += sub_length; 914 memAddr = 0; 915 } 916 result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer); 917 if (result) { 918 LOG_RESULT_LOCATION(result); 919 return result; 920 } 921 return result; 922 } 923 924 /** 925 * @brief Load the DMP with the given code and configuration. 926 * @param buffer 927 * the DMP data. 928 * @param length 929 * the length in bytes of the DMP data. 930 * @param config 931 * the DMP configuration. 932 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 933 */ 934 inv_error_t inv_load_dmp(const unsigned char *buffer, 935 unsigned short length, unsigned short config) 936 { 937 INVENSENSE_FUNC_START; 938 939 inv_error_t result = INV_SUCCESS; 940 unsigned short toWrite; 941 unsigned short memAddr = 0; 942 localDmpMemory = buffer; 943 localDmpMemorySize = length; 944 945 mldlCfg.dmp_cfg1 = (config >> 8); 946 mldlCfg.dmp_cfg2 = (config & 0xff); 947 948 while (length > 0) { 949 toWrite = length; 950 if (toWrite > MAX_LOAD_WRITE_SIZE) 951 toWrite = MAX_LOAD_WRITE_SIZE; 952 953 result = 954 inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite, 955 buffer); 956 if (result) { 957 LOG_RESULT_LOCATION(result); 958 return result; 959 } 960 961 buffer += toWrite; 962 memAddr += toWrite; 963 length -= toWrite; 964 } 965 966 return result; 967 } 968 969 /** 970 * @brief Get the silicon revision ID. 971 * @return The silicon revision ID 972 * (0 will be read if inv_mpu_open returned an error) 973 */ 974 unsigned char inv_get_silicon_rev(void) 975 { 976 return mldlCfg.silicon_revision; 977 } 978 979 /** 980 * @brief Get the product revision ID. 981 * @return The product revision ID 982 * (0 will be read if inv_mpu_open returned an error) 983 */ 984 unsigned char inv_get_product_rev(void) 985 { 986 return mldlCfg.product_revision; 987 } 988 989 /******************************************************************************* 990 ******************************************************************************* 991 ******************************************************************************* 992 * @todo these belong with an interface to the kernel driver layer 993 ******************************************************************************* 994 ******************************************************************************* 995 ******************************************************************************/ 996 997 /** 998 * @brief inv_get_interrupt_status returns the interrupt status from the specified 999 * interrupt pin. 1000 * @param intPin 1001 * Currently only the value INTPIN_MPU is supported. 1002 * @param status 1003 * The available statuses are: 1004 * - BIT_MPU_RDY_EN 1005 * - BIT_DMP_INT_EN 1006 * - BIT_RAW_RDY_EN 1007 * 1008 * @return INV_SUCCESS or a non-zero error code. 1009 */ 1010 inv_error_t inv_get_interrupt_status(unsigned char intPin, 1011 unsigned char *status) 1012 { 1013 INVENSENSE_FUNC_START; 1014 1015 inv_error_t result; 1016 1017 switch (intPin) { 1018 1019 case INTPIN_MPU: 1020 /*---- return the MPU interrupt status ----*/ 1021 result = inv_serial_read(sMLSLHandle, mldlCfg.addr, 1022 MPUREG_INT_STATUS, 1, status); 1023 break; 1024 1025 default: 1026 result = INV_ERROR_INVALID_PARAMETER; 1027 break; 1028 } 1029 1030 return result; 1031 } 1032 1033 /** 1034 * @brief query the current status of an interrupt source. 1035 * @param srcIndex 1036 * index of the interrupt source. 1037 * Currently the only source supported is INTPIN_MPU. 1038 * 1039 * @return 1 if the interrupt has been triggered. 1040 */ 1041 unsigned char inv_get_interrupt_trigger(unsigned char srcIndex) 1042 { 1043 INVENSENSE_FUNC_START; 1044 return intTrigger[srcIndex]; 1045 } 1046 1047 /** 1048 * @brief clear the 'triggered' status for an interrupt source. 1049 * @param srcIndex 1050 * index of the interrupt source. 1051 * Currently only INTPIN_MPU is supported. 1052 */ 1053 void inv_clear_interrupt_trigger(unsigned char srcIndex) 1054 { 1055 INVENSENSE_FUNC_START; 1056 intTrigger[srcIndex] = 0; 1057 } 1058 1059 /** 1060 * @brief inv_interrupt_handler function should be called when an interrupt is 1061 * received. The source parameter identifies which interrupt source 1062 * caused the interrupt. Note that this routine should not be called 1063 * directly from the interrupt service routine. 1064 * 1065 * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1, 1066 * INTSRC_AUX2, or INT_SRC_TIMER. 1067 * 1068 * @return Zero if the command is successful; an error code otherwise. 1069 */ 1070 inv_error_t inv_interrupt_handler(unsigned char intSource) 1071 { 1072 INVENSENSE_FUNC_START; 1073 /*---- range check ----*/ 1074 if (intSource >= NUM_OF_INTSOURCES) { 1075 return INV_ERROR; 1076 } 1077 1078 /*---- save source of interrupt ----*/ 1079 intTrigger[intSource] = INT_TRIGGERED; 1080 1081 #ifdef ML_USE_DMP_SIM 1082 if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { 1083 MLSimHWDataInput(); 1084 } 1085 #endif 1086 1087 return INV_SUCCESS; 1088 } 1089 1090 /***************************/ 1091 /**@}*//* end of defgroup */ 1092 /***************************/ 1093
