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: ml.c 5642 2011-06-14 02:26:20Z mcaramello $ 21 * 22 *****************************************************************************/ 23 24 /** 25 * @defgroup ML 26 * @brief Motion Library APIs. 27 * The Motion Library processes gyroscopes, accelerometers, and 28 * compasses to provide a physical model of the movement for the 29 * sensors. 30 * The results of this processing may be used to control objects 31 * within a user interface environment, detect gestures, track 3D 32 * movement for gaming applications, and analyze the blur created 33 * due to hand movement while taking a picture. 34 * 35 * @{ 36 * @file ml.c 37 * @brief The Motion Library APIs. 38 */ 39 40 /* ------------------ */ 41 /* - Include Files. - */ 42 /* ------------------ */ 43 44 #include <string.h> 45 46 #include "ml.h" 47 #include "mldl.h" 48 #include "mltypes.h" 49 #include "mlinclude.h" 50 #include "compass.h" 51 #include "dmpKey.h" 52 #include "dmpDefault.h" 53 #include "mlstates.h" 54 #include "mlFIFO.h" 55 #include "mlFIFOHW.h" 56 #include "mlMathFunc.h" 57 #include "mlsupervisor.h" 58 #include "mlmath.h" 59 #include "mlcontrol.h" 60 #include "mldl_cfg.h" 61 #include "mpu.h" 62 #include "accel.h" 63 #include "mlos.h" 64 #include "mlsl.h" 65 #include "mlos.h" 66 #include "mlBiasNoMotion.h" 67 #include "log.h" 68 #undef MPL_LOG_TAG 69 #define MPL_LOG_TAG "MPL-ml" 70 71 #define ML_MOT_TYPE_NONE 0 72 #define ML_MOT_TYPE_NO_MOTION 1 73 #define ML_MOT_TYPE_MOTION_DETECTED 2 74 75 #define ML_MOT_STATE_MOVING 0 76 #define ML_MOT_STATE_NO_MOTION 1 77 #define ML_MOT_STATE_BIAS_IN_PROG 2 78 79 #define _mlDebug(x) //{x} 80 81 /* Global Variables */ 82 const unsigned char mlVer[] = { INV_VERSION }; 83 84 struct inv_params_obj inv_params_obj = { 85 INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode 86 INV_ORIENTATION_MASK_DEFAULT, // orientation_mask 87 INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func 88 INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func 89 INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func 90 INV_STATE_SERIAL_CLOSED // starting state 91 }; 92 93 struct inv_obj_t inv_obj; 94 void *g_mlsl_handle; 95 96 typedef struct { 97 // These describe callbacks happening everythime a DMP interrupt is processed 98 int_fast8_t numInterruptProcesses; 99 // Array of function pointers, function being void function taking void 100 inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES]; 101 102 } tMLXCallbackInterrupt; // MLX_callback_t 103 104 tMLXCallbackInterrupt mlxCallbackInterrupt; 105 106 /* --------------- */ 107 /* - Functions. - */ 108 /* --------------- */ 109 110 inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient); 111 inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient); 112 unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); 113 114 /** 115 * @brief Open serial connection with the MPU device. 116 * This is the entry point of the MPL and must be 117 * called prior to any other function call. 118 * 119 * @param port System handle for 'port' MPU device is found on. 120 * The significance of this parameter varies by 121 * platform. It is passed as 'port' to MLSLSerialOpen. 122 * 123 * @return INV_SUCCESS or error code. 124 */ 125 inv_error_t inv_serial_start(char const *port) 126 { 127 INVENSENSE_FUNC_START; 128 inv_error_t result; 129 130 if (inv_get_state() >= INV_STATE_SERIAL_OPENED) 131 return INV_SUCCESS; 132 133 result = inv_state_transition(INV_STATE_SERIAL_OPENED); 134 if (result) { 135 LOG_RESULT_LOCATION(result); 136 return result; 137 } 138 139 result = inv_serial_open(port, &g_mlsl_handle); 140 if (INV_SUCCESS != result) { 141 (void)inv_state_transition(INV_STATE_SERIAL_CLOSED); 142 } 143 144 return result; 145 } 146 147 /** 148 * @brief Close the serial communication. 149 * This function needs to be called explicitly to shut down 150 * the communication with the device. Calling inv_dmp_close() 151 * won't affect the established serial communication. 152 * @return INV_SUCCESS; non-zero error code otherwise. 153 */ 154 inv_error_t inv_serial_stop(void) 155 { 156 INVENSENSE_FUNC_START; 157 inv_error_t result = INV_SUCCESS; 158 159 if (inv_get_state() == INV_STATE_SERIAL_CLOSED) 160 return INV_SUCCESS; 161 162 result = inv_state_transition(INV_STATE_SERIAL_CLOSED); 163 if (INV_SUCCESS != result) { 164 MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result); 165 } 166 result = inv_serial_close(g_mlsl_handle); 167 if (INV_SUCCESS != result) { 168 MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result); 169 } 170 return result; 171 } 172 173 /** 174 * @brief Get the serial file handle to the device. 175 * @return The serial file handle. 176 */ 177 void *inv_get_serial_handle(void) 178 { 179 INVENSENSE_FUNC_START; 180 return g_mlsl_handle; 181 } 182 183 /** 184 * @brief apply the choosen orientation and full scale range 185 * for gyroscopes, accelerometer, and compass. 186 * @return INV_SUCCESS if successful, a non-zero code otherwise. 187 */ 188 inv_error_t inv_apply_calibration(void) 189 { 190 INVENSENSE_FUNC_START; 191 signed char gyroCal[9] = { 0 }; 192 signed char accelCal[9] = { 0 }; 193 signed char magCal[9] = { 0 }; 194 float gyroScale = 2000.f; 195 float accelScale = 0.f; 196 float magScale = 0.f; 197 198 inv_error_t result; 199 int ii; 200 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 201 202 for (ii = 0; ii < 9; ii++) { 203 gyroCal[ii] = mldl_cfg->pdata->orientation[ii]; 204 accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii]; 205 magCal[ii] = mldl_cfg->pdata->compass.orientation[ii]; 206 } 207 208 switch (mldl_cfg->full_scale) { 209 case MPU_FS_250DPS: 210 gyroScale = 250.f; 211 break; 212 case MPU_FS_500DPS: 213 gyroScale = 500.f; 214 break; 215 case MPU_FS_1000DPS: 216 gyroScale = 1000.f; 217 break; 218 case MPU_FS_2000DPS: 219 gyroScale = 2000.f; 220 break; 221 default: 222 MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n", 223 mldl_cfg->full_scale); 224 return INV_ERROR_INVALID_PARAMETER; 225 break; 226 } 227 228 RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale); 229 inv_obj.accel_sens = (long)(accelScale * 65536L); 230 /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */ 231 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 232 defined CONFIG_MPU_SENSORS_MPU6050B1 233 inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim; 234 #else 235 inv_obj.accel_sens /= 2; 236 #endif 237 238 RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale); 239 inv_obj.compass_sens = (long)(magScale * 32768); 240 241 if (inv_get_state() == INV_STATE_DMP_OPENED) { 242 result = inv_set_gyro_calibration(gyroScale, gyroCal); 243 if (INV_SUCCESS != result) { 244 MPL_LOGE("Unable to set Gyro Calibration\n"); 245 return result; 246 } 247 result = inv_set_accel_calibration(accelScale, accelCal); 248 if (INV_SUCCESS != result) { 249 MPL_LOGE("Unable to set Accel Calibration\n"); 250 return result; 251 } 252 result = inv_set_compass_calibration(magScale, magCal); 253 if (INV_SUCCESS != result) { 254 MPL_LOGE("Unable to set Mag Calibration\n"); 255 return result; 256 } 257 } 258 return INV_SUCCESS; 259 } 260 261 /** 262 * @brief Setup the DMP to handle the accelerometer endianess. 263 * @return INV_SUCCESS if successful, a non-zero error code otherwise. 264 */ 265 inv_error_t inv_apply_endian_accel(void) 266 { 267 INVENSENSE_FUNC_START; 268 unsigned char regs[4] = { 0 }; 269 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 270 int endian = mldl_cfg->accel->endian; 271 272 if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) { 273 endian = EXT_SLAVE_BIG_ENDIAN; 274 } 275 switch (endian) { 276 case EXT_SLAVE_FS8_BIG_ENDIAN: 277 case EXT_SLAVE_FS16_BIG_ENDIAN: 278 case EXT_SLAVE_LITTLE_ENDIAN: 279 regs[0] = 0; 280 regs[1] = 64; 281 regs[2] = 0; 282 regs[3] = 0; 283 break; 284 case EXT_SLAVE_BIG_ENDIAN: 285 default: 286 regs[0] = 0; 287 regs[1] = 0; 288 regs[2] = 64; 289 regs[3] = 0; 290 } 291 292 return inv_set_mpu_memory(KEY_D_1_236, 4, regs); 293 } 294 295 /** 296 * @internal 297 * @brief Initialize MLX data. This should be called to setup the mlx 298 * output buffers before any motion processing is done. 299 */ 300 void inv_init_ml(void) 301 { 302 INVENSENSE_FUNC_START; 303 int ii; 304 long tmp[COMPASS_NUM_AXES]; 305 // Set all values to zero by default 306 memset(&inv_obj, 0, sizeof(struct inv_obj_t)); 307 memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt)); 308 309 inv_obj.compass_correction[0] = 1073741824L; 310 inv_obj.compass_correction_relative[0] = 1073741824L; 311 inv_obj.compass_disturb_correction[0] = 1073741824L; 312 inv_obj.compass_correction_offset[0] = 1073741824L; 313 inv_obj.relative_quat[0] = 1073741824L; 314 315 //Not used with the ST accelerometer 316 inv_obj.no_motion_threshold = 20; // noMotionThreshold 317 //Not used with the ST accelerometer 318 inv_obj.motion_duration = 1536; // motionDuration 319 320 inv_obj.motion_state = INV_MOTION; // Motion state 321 322 inv_obj.bias_update_time = 8000; 323 inv_obj.bias_calc_time = 2000; 324 325 inv_obj.internal_motion_state = ML_MOT_STATE_MOVING; 326 327 inv_obj.start_time = inv_get_tick_count(); 328 329 inv_obj.compass_cal[0] = 322122560L; 330 inv_obj.compass_cal[4] = 322122560L; 331 inv_obj.compass_cal[8] = 322122560L; 332 inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed. 333 334 for (ii = 0; ii < COMPASS_NUM_AXES; ii++) { 335 inv_obj.compass_scale[ii] = 65536L; 336 inv_obj.compass_test_scale[ii] = 65536L; 337 inv_obj.compass_bias_error[ii] = P_INIT; 338 inv_obj.init_compass_bias[ii] = 0; 339 inv_obj.compass_asa[ii] = (1L << 30); 340 } 341 if (inv_compass_read_scale(tmp) == INV_SUCCESS) { 342 for (ii = 0; ii < COMPASS_NUM_AXES; ii++) 343 inv_obj.compass_asa[ii] = tmp[ii]; 344 } 345 inv_obj.got_no_motion_bias = 0; 346 inv_obj.got_compass_bias = 0; 347 inv_obj.compass_state = SF_UNCALIBRATED; 348 inv_obj.acc_state = SF_STARTUP_SETTLE; 349 350 inv_obj.got_init_compass_bias = 0; 351 inv_obj.resetting_compass = 0; 352 353 inv_obj.external_slave_callback = NULL; 354 inv_obj.compass_accuracy = 0; 355 356 inv_obj.factory_temp_comp = 0; 357 inv_obj.got_coarse_heading = 0; 358 359 inv_obj.compass_bias_ptr[0] = P_INIT; 360 inv_obj.compass_bias_ptr[4] = P_INIT; 361 inv_obj.compass_bias_ptr[8] = P_INIT; 362 363 inv_obj.gyro_bias_err = 1310720; 364 365 inv_obj.accel_lpf_gain = 1073744L; 366 inv_obj.no_motion_accel_threshold = 7000000L; 367 } 368 369 /** 370 * @internal 371 * @brief This registers a function to be called for each time the DMP 372 * generates an an interrupt. 373 * It will be called after inv_register_fifo_rate_process() which gets called 374 * every time the FIFO data is processed. 375 * The FIFO does not have to be on for this callback. 376 * @param func Function to be called when a DMP interrupt occurs. 377 * @return INV_SUCCESS or non-zero error code. 378 */ 379 inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func) 380 { 381 INVENSENSE_FUNC_START; 382 // Make sure we have not filled up our number of allowable callbacks 383 if (mlxCallbackInterrupt.numInterruptProcesses <= 384 MAX_INTERRUPT_PROCESSES - 1) { 385 int kk; 386 // Make sure we haven't registered this function already 387 for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { 388 if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { 389 return INV_ERROR_INVALID_PARAMETER; 390 } 391 } 392 // Add new callback 393 mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt. 394 numInterruptProcesses] = func; 395 mlxCallbackInterrupt.numInterruptProcesses++; 396 return INV_SUCCESS; 397 } 398 return INV_ERROR_MEMORY_EXAUSTED; 399 } 400 401 /** 402 * @internal 403 * @brief This unregisters a function to be called for each DMP interrupt. 404 * @return INV_SUCCESS or non-zero error code. 405 */ 406 inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func) 407 { 408 INVENSENSE_FUNC_START; 409 int kk, jj; 410 // Make sure we haven't registered this function already 411 for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { 412 if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { 413 // FIXME, we may need a thread block here 414 for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses; 415 ++jj) { 416 mlxCallbackInterrupt.processInterruptCb[jj - 1] = 417 mlxCallbackInterrupt.processInterruptCb[jj]; 418 } 419 mlxCallbackInterrupt.numInterruptProcesses--; 420 return INV_SUCCESS; 421 } 422 } 423 return INV_ERROR_INVALID_PARAMETER; 424 } 425 426 /** 427 * @internal 428 * @brief Run the recorded interrupt process callbacks in the event 429 * of an interrupt. 430 */ 431 void inv_run_dmp_interupt_cb(void) 432 { 433 int kk; 434 for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { 435 if (mlxCallbackInterrupt.processInterruptCb[kk]) 436 mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj); 437 } 438 } 439 440 /** @internal 441 * Resets the Motion/No Motion state which should be called at startup and resume. 442 */ 443 inv_error_t inv_reset_motion(void) 444 { 445 unsigned char regs[8]; 446 inv_error_t result; 447 448 inv_obj.motion_state = INV_MOTION; 449 inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; 450 inv_obj.no_motion_accel_time = inv_get_tick_count(); 451 #if defined CONFIG_MPU_SENSORS_MPU3050 452 regs[0] = DINAD8 + 2; 453 regs[1] = DINA0C; 454 regs[2] = DINAD8 + 1; 455 result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); 456 if (result) { 457 LOG_RESULT_LOCATION(result); 458 return result; 459 } 460 #else 461 #endif 462 regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); 463 regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); 464 result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); 465 if (result) { 466 LOG_RESULT_LOCATION(result); 467 return result; 468 } 469 memset(regs, 0, 8); 470 result = inv_set_mpu_memory(KEY_D_1_96, 8, regs); 471 if (result) { 472 LOG_RESULT_LOCATION(result); 473 return result; 474 } 475 476 result = 477 inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs)); 478 if (result) { 479 LOG_RESULT_LOCATION(result); 480 return result; 481 } 482 483 inv_set_motion_state(INV_MOTION); 484 return result; 485 } 486 487 /** 488 * @internal 489 * @brief inv_start_bias_calc starts the bias calculation on the MPU. 490 */ 491 void inv_start_bias_calc(void) 492 { 493 INVENSENSE_FUNC_START; 494 495 inv_obj.suspend = 1; 496 } 497 498 /** 499 * @internal 500 * @brief inv_stop_bias_calc stops the bias calculation on the MPU. 501 */ 502 void inv_stop_bias_calc(void) 503 { 504 INVENSENSE_FUNC_START; 505 506 inv_obj.suspend = 0; 507 } 508 509 /** 510 * @brief inv_update_data fetches data from the fifo and updates the 511 * motion algorithms. 512 * 513 * @pre inv_dmp_open() 514 * @ifnot MPL_MF 515 * or inv_open_low_power_pedometer() 516 * or inv_eis_open_dmp() 517 * @endif 518 * and inv_dmp_start() must have been called. 519 * 520 * @note Motion algorithm data is constant between calls to inv_update_data 521 * 522 * @return 523 * - INV_SUCCESS 524 * - Non-zero error code 525 */ 526 inv_error_t inv_update_data(void) 527 { 528 INVENSENSE_FUNC_START; 529 inv_error_t result = INV_SUCCESS; 530 int_fast8_t got, ftry; 531 uint_fast8_t mpu_interrupt; 532 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 533 534 if (inv_get_state() != INV_STATE_DMP_STARTED) 535 return INV_ERROR_SM_IMPROPER_STATE; 536 537 // Set the maximum number of FIFO packets we want to process 538 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { 539 ftry = 100; // Large enough to process all packets 540 } else { 541 ftry = 1; 542 } 543 544 // Go and process at most ftry number of packets, probably less than ftry 545 result = inv_read_and_process_fifo(ftry, &got); 546 if (result) { 547 LOG_RESULT_LOCATION(result); 548 return result; 549 } 550 551 // Process all interrupts 552 mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1); 553 if (mpu_interrupt) { 554 inv_clear_interrupt_trigger(INTSRC_AUX1); 555 } 556 // Check if interrupt was from MPU 557 mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU); 558 if (mpu_interrupt) { 559 inv_clear_interrupt_trigger(INTSRC_MPU); 560 } 561 // Take care of the callbacks that want to be notified when there was an MPU interrupt 562 if (mpu_interrupt) { 563 inv_run_dmp_interupt_cb(); 564 } 565 566 result = inv_get_fifo_status(); 567 return result; 568 } 569 570 /** 571 * @brief inv_check_flag returns the value of a flag. 572 * inv_check_flag can be used to check a number of flags, 573 * allowing users to poll flags rather than register callback 574 * functions. If a flag is set to True when inv_check_flag is called, 575 * the flag is automatically reset. 576 * The flags are: 577 * - INV_RAW_DATA_READY 578 * Indicates that new raw data is available. 579 * - INV_PROCESSED_DATA_READY 580 * Indicates that new processed data is available. 581 * - INV_GOT_GESTURE 582 * Indicates that a gesture has been detected by the gesture engine. 583 * - INV_MOTION_STATE_CHANGE 584 * Indicates that a change has been made from motion to no motion, 585 * or vice versa. 586 * 587 * @pre inv_dmp_open() 588 * @ifnot MPL_MF 589 * or inv_open_low_power_pedometer() 590 * or inv_eis_open_dmp() 591 * @endif 592 * and inv_dmp_start() must have been called. 593 * 594 * @param flag The flag to check. 595 * 596 * @return TRUE or FALSE state of the flag 597 */ 598 int inv_check_flag(int flag) 599 { 600 INVENSENSE_FUNC_START; 601 int flagReturn = inv_obj.flags[flag]; 602 603 inv_obj.flags[flag] = 0; 604 return flagReturn; 605 } 606 607 /** 608 * @brief Enable generation of the DMP interrupt when Motion or no-motion 609 * is detected 610 * @param on 611 * Boolean to turn the interrupt on or off. 612 * @return INV_SUCCESS or non-zero error code. 613 */ 614 inv_error_t inv_set_motion_interrupt(unsigned char on) 615 { 616 INVENSENSE_FUNC_START; 617 inv_error_t result; 618 unsigned char regs[2] = { 0 }; 619 620 if (inv_get_state() < INV_STATE_DMP_OPENED) 621 return INV_ERROR_SM_IMPROPER_STATE; 622 623 if (on) { 624 result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); 625 if (result) { 626 LOG_RESULT_LOCATION(result); 627 return result; 628 } 629 inv_obj.interrupt_sources |= INV_INT_MOTION; 630 } else { 631 inv_obj.interrupt_sources &= ~INV_INT_MOTION; 632 if (!inv_obj.interrupt_sources) { 633 result = inv_get_dl_cfg_int(0); 634 if (result) { 635 LOG_RESULT_LOCATION(result); 636 return result; 637 } 638 } 639 } 640 641 if (on) { 642 regs[0] = DINAFE; 643 } else { 644 regs[0] = DINAD8; 645 } 646 result = inv_set_mpu_memory(KEY_CFG_7, 1, regs); 647 if (result) { 648 LOG_RESULT_LOCATION(result); 649 return result; 650 } 651 return result; 652 } 653 654 /** 655 * Enable generation of the DMP interrupt when a FIFO packet is ready 656 * 657 * @param on Boolean to turn the interrupt on or off 658 * 659 * @return INV_SUCCESS or non-zero error code 660 */ 661 inv_error_t inv_set_fifo_interrupt(unsigned char on) 662 { 663 INVENSENSE_FUNC_START; 664 inv_error_t result; 665 unsigned char regs[2] = { 0 }; 666 667 if (inv_get_state() < INV_STATE_DMP_OPENED) 668 return INV_ERROR_SM_IMPROPER_STATE; 669 670 if (on) { 671 result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); 672 if (result) { 673 LOG_RESULT_LOCATION(result); 674 return result; 675 } 676 inv_obj.interrupt_sources |= INV_INT_FIFO; 677 } else { 678 inv_obj.interrupt_sources &= ~INV_INT_FIFO; 679 if (!inv_obj.interrupt_sources) { 680 result = inv_get_dl_cfg_int(0); 681 if (result) { 682 LOG_RESULT_LOCATION(result); 683 return result; 684 } 685 } 686 } 687 688 if (on) { 689 regs[0] = DINAFE; 690 } else { 691 regs[0] = DINAD8; 692 } 693 result = inv_set_mpu_memory(KEY_CFG_6, 1, regs); 694 if (result) { 695 LOG_RESULT_LOCATION(result); 696 return result; 697 } 698 return result; 699 } 700 701 /** 702 * @brief Get the current set of DMP interrupt sources. 703 * These interrupts are generated by the DMP and can be 704 * routed to the MPU interrupt line via internal 705 * settings. 706 * 707 * @pre inv_dmp_open() 708 * @ifnot MPL_MF 709 * or inv_open_low_power_pedometer() 710 * or inv_eis_open_dmp() 711 * @endif 712 * must have been called. 713 * 714 * @return Currently enabled interrupt sources. The possible interrupts are: 715 * - INV_INT_FIFO, 716 * - INV_INT_MOTION, 717 * - INV_INT_TAP 718 */ 719 int inv_get_interrupts(void) 720 { 721 INVENSENSE_FUNC_START; 722 723 if (inv_get_state() < INV_STATE_DMP_OPENED) 724 return 0; // error 725 726 return inv_obj.interrupt_sources; 727 } 728 729 /** 730 * @brief Sets up the Accelerometer calibration and scale factor. 731 * 732 * Please refer to the provided "9-Axis Sensor Fusion Application 733 * Note" document provided. Section 5, "Sensor Mounting Orientation" 734 * offers a good coverage on the mounting matrices and explains how 735 * to use them. 736 * 737 * @pre inv_dmp_open() 738 * @ifnot MPL_MF 739 * or inv_open_low_power_pedometer() 740 * or inv_eis_open_dmp() 741 * @endif 742 * must have been called. 743 * @pre inv_dmp_start() must <b>NOT</b> have been called. 744 * 745 * @see inv_set_gyro_calibration(). 746 * @see inv_set_compass_calibration(). 747 * 748 * @param[in] range 749 * The range of the accelerometers in g's. An accelerometer 750 * that has a range of +2g's to -2g's should pass in 2. 751 * @param[in] orientation 752 * A 9 element matrix that represents how the accelerometers 753 * are oriented with respect to the device they are mounted 754 * in and the reference axis system. 755 * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. 756 * This example corresponds to a 3 x 3 identity matrix. 757 * 758 * @return INV_SUCCESS if successful; a non-zero error code otherwise. 759 */ 760 inv_error_t inv_set_accel_calibration(float range, signed char *orientation) 761 { 762 INVENSENSE_FUNC_START; 763 float cal[9]; 764 float scale = range / 32768.f; 765 int kk; 766 unsigned long sf; 767 inv_error_t result; 768 unsigned char regs[4] = { 0, 0, 0, 0 }; 769 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 770 771 if (inv_get_state() != INV_STATE_DMP_OPENED) 772 return INV_ERROR_SM_IMPROPER_STATE; 773 774 /* Apply zero g-offset values */ 775 if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) { 776 regs[0] = 0x80; 777 regs[1] = 0x0; 778 regs[2] = 0x80; 779 regs[3] = 0x0; 780 } 781 782 if (inv_dmpkey_supported(KEY_D_1_152)) { 783 result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]); 784 if (result) { 785 LOG_RESULT_LOCATION(result); 786 return result; 787 } 788 } 789 790 if (scale == 0) { 791 inv_obj.accel_sens = 0; 792 } 793 794 if (mldl_cfg->accel->id) { 795 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 796 defined CONFIG_MPU_SENSORS_MPU6050B1 797 unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C }; 798 #else 799 unsigned char tmp[3] = { DINA4C, DINACD, DINA6C }; 800 #endif 801 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 802 unsigned char regs[3]; 803 unsigned short orient; 804 805 for (kk = 0; kk < 9; ++kk) { 806 cal[kk] = scale * orientation[kk]; 807 inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); 808 } 809 810 orient = inv_orientation_matrix_to_scalar(orientation); 811 regs[0] = tmp[orient & 3]; 812 regs[1] = tmp[(orient >> 3) & 3]; 813 regs[2] = tmp[(orient >> 6) & 3]; 814 result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs); 815 if (result) { 816 LOG_RESULT_LOCATION(result); 817 return result; 818 } 819 820 regs[0] = DINA26; 821 regs[1] = DINA46; 822 #if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2 823 regs[2] = DINA66; 824 #else 825 if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision) 826 == MPU_PRODUCT_KEY_B1_E1_5) 827 regs[2] = DINA76; 828 else 829 regs[2] = DINA66; 830 #endif 831 if (orient & 4) 832 regs[0] |= 1; 833 if (orient & 0x20) 834 regs[1] |= 1; 835 if (orient & 0x100) 836 regs[2] |= 1; 837 838 result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs); 839 if (result) { 840 LOG_RESULT_LOCATION(result); 841 return result; 842 } 843 844 if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) { 845 result = inv_freescale_sensor_fusion_16bit(orient); 846 if (result) { 847 LOG_RESULT_LOCATION(result); 848 return result; 849 } 850 } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) { 851 result = inv_freescale_sensor_fusion_8bit(orient); 852 if (result) { 853 LOG_RESULT_LOCATION(result); 854 return result; 855 } 856 } 857 } 858 859 if (inv_obj.accel_sens != 0) { 860 sf = (1073741824L / inv_obj.accel_sens); 861 } else { 862 sf = 0; 863 } 864 regs[0] = (unsigned char)((sf >> 8) & 0xff); 865 regs[1] = (unsigned char)(sf & 0xff); 866 result = inv_set_mpu_memory(KEY_D_0_108, 2, regs); 867 if (result) { 868 LOG_RESULT_LOCATION(result); 869 return result; 870 } 871 872 return result; 873 } 874 875 /** 876 * @brief Sets up the Gyro calibration and scale factor. 877 * 878 * Please refer to the provided "9-Axis Sensor Fusion Application 879 * Note" document provided. Section 5, "Sensor Mounting Orientation" 880 * offers a good coverage on the mounting matrices and explains 881 * how to use them. 882 * 883 * @pre inv_dmp_open() 884 * @ifnot MPL_MF 885 * or inv_open_low_power_pedometer() 886 * or inv_eis_open_dmp() 887 * @endif 888 * must have been called. 889 * @pre inv_dmp_start() must have <b>NOT</b> been called. 890 * 891 * @see inv_set_accel_calibration(). 892 * @see inv_set_compass_calibration(). 893 * 894 * @param[in] range 895 * The range of the gyros in degrees per second. A gyro 896 * that has a range of +2000 dps to -2000 dps should pass in 897 * 2000. 898 * @param[in] orientation 899 * A 9 element matrix that represents how the gyro are oriented 900 * with respect to the device they are mounted in. A typical 901 * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This 902 * example corresponds to a 3 x 3 identity matrix. 903 * 904 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 905 */ 906 inv_error_t inv_set_gyro_calibration(float range, signed char *orientation) 907 { 908 INVENSENSE_FUNC_START; 909 910 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 911 int kk; 912 float scale; 913 inv_error_t result; 914 915 unsigned char regs[12] = { 0 }; 916 unsigned char maxVal = 0; 917 unsigned char tmpPtr = 0; 918 unsigned char tmpSign = 0; 919 unsigned char i = 0; 920 int sf = 0; 921 922 if (inv_get_state() != INV_STATE_DMP_OPENED) 923 return INV_ERROR_SM_IMPROPER_STATE; 924 925 if (mldl_cfg->gyro_sens_trim != 0) { 926 /* adjust the declared range to consider the gyro_sens_trim 927 of this part when different from the default (first dividend) */ 928 range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim; 929 } 930 931 scale = range / 32768.f; // inverse of sensitivity for the given full scale range 932 inv_obj.gyro_sens = (long)(range * 32768L); 933 934 for (kk = 0; kk < 9; ++kk) { 935 inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30)); 936 inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30)); 937 } 938 939 { 940 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 941 defined CONFIG_MPU_SENSORS_MPU6050B1 942 unsigned char tmpD = DINA4C; 943 unsigned char tmpE = DINACD; 944 unsigned char tmpF = DINA6C; 945 #else 946 unsigned char tmpD = DINAC9; 947 unsigned char tmpE = DINA2C; 948 unsigned char tmpF = DINACB; 949 #endif 950 regs[3] = DINA36; 951 regs[4] = DINA56; 952 regs[5] = DINA76; 953 954 for (i = 0; i < 3; i++) { 955 maxVal = 0; 956 tmpSign = 0; 957 if (inv_obj.gyro_orient[0 + 3 * i] < 0) 958 tmpSign = 1; 959 if (ABS(inv_obj.gyro_orient[1 + 3 * i]) > 960 ABS(inv_obj.gyro_orient[0 + 3 * i])) { 961 maxVal = 1; 962 if (inv_obj.gyro_orient[1 + 3 * i] < 0) 963 tmpSign = 1; 964 } 965 if (ABS(inv_obj.gyro_orient[2 + 3 * i]) > 966 ABS(inv_obj.gyro_orient[1 + 3 * i])) { 967 tmpSign = 0; 968 maxVal = 2; 969 if (inv_obj.gyro_orient[2 + 3 * i] < 0) 970 tmpSign = 1; 971 } 972 if (maxVal == 0) { 973 regs[tmpPtr++] = tmpD; 974 if (tmpSign) 975 regs[tmpPtr + 2] |= 0x01; 976 } else if (maxVal == 1) { 977 regs[tmpPtr++] = tmpE; 978 if (tmpSign) 979 regs[tmpPtr + 2] |= 0x01; 980 } else { 981 regs[tmpPtr++] = tmpF; 982 if (tmpSign) 983 regs[tmpPtr + 2] |= 0x01; 984 } 985 } 986 result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs); 987 if (result) { 988 LOG_RESULT_LOCATION(result); 989 return result; 990 } 991 result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]); 992 if (result) { 993 LOG_RESULT_LOCATION(result); 994 return result; 995 } 996 997 //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384 998 inv_obj.gyro_sf = 999 (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL); 1000 result = 1001 inv_set_mpu_memory(KEY_D_0_104, 4, 1002 inv_int32_to_big8(inv_obj.gyro_sf, regs)); 1003 if (result) { 1004 LOG_RESULT_LOCATION(result); 1005 return result; 1006 } 1007 1008 if (inv_obj.gyro_sens != 0) { 1009 sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens); 1010 } else { 1011 sf = 0; 1012 } 1013 1014 result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs)); 1015 if (result) { 1016 LOG_RESULT_LOCATION(result); 1017 return result; 1018 } 1019 } 1020 return INV_SUCCESS; 1021 } 1022 1023 /** 1024 * @brief Sets up the Compass calibration and scale factor. 1025 * 1026 * Please refer to the provided "9-Axis Sensor Fusion Application 1027 * Note" document provided. Section 5, "Sensor Mounting Orientation" 1028 * offers a good coverage on the mounting matrices and explains 1029 * how to use them. 1030 * 1031 * @pre inv_dmp_open() 1032 * @ifnot MPL_MF 1033 * or inv_open_low_power_pedometer() 1034 * or inv_eis_open_dmp() 1035 * @endif 1036 * must have been called. 1037 * @pre inv_dmp_start() must have <b>NOT</b> been called. 1038 * 1039 * @see inv_set_gyro_calibration(). 1040 * @see inv_set_accel_calibration(). 1041 * 1042 * @param[in] range 1043 * The range of the compass. 1044 * @param[in] orientation 1045 * A 9 element matrix that represents how the compass is 1046 * oriented with respect to the device they are mounted in. 1047 * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. 1048 * This example corresponds to a 3 x 3 identity matrix. 1049 * The matrix describes how to go from the chip mounting to 1050 * the body of the device. 1051 * 1052 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1053 */ 1054 inv_error_t inv_set_compass_calibration(float range, signed char *orientation) 1055 { 1056 INVENSENSE_FUNC_START; 1057 float cal[9]; 1058 float scale = range / 32768.f; 1059 int kk; 1060 unsigned short compassId = 0; 1061 1062 compassId = inv_get_compass_id(); 1063 1064 if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883) 1065 || (compassId == COMPASS_ID_LSM303M)) { 1066 scale /= 32.0f; 1067 } 1068 1069 for (kk = 0; kk < 9; ++kk) { 1070 cal[kk] = scale * orientation[kk]; 1071 inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); 1072 } 1073 1074 inv_obj.compass_sens = (long)(scale * 1073741824L); 1075 1076 if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) { 1077 unsigned char reg0[4] = { 0, 0, 0, 0 }; 1078 unsigned char regp[4] = { 64, 0, 0, 0 }; 1079 unsigned char regn[4] = { 64 + 128, 0, 0, 0 }; 1080 unsigned char *reg; 1081 int_fast8_t kk; 1082 unsigned short keyList[9] = 1083 { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02, 1084 KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12, 1085 KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22 1086 }; 1087 1088 for (kk = 0; kk < 9; ++kk) { 1089 1090 if (orientation[kk] == 1) 1091 reg = regp; 1092 else if (orientation[kk] == -1) 1093 reg = regn; 1094 else 1095 reg = reg0; 1096 inv_set_mpu_memory(keyList[kk], 4, reg); 1097 } 1098 } 1099 1100 return INV_SUCCESS; 1101 } 1102 1103 /** 1104 * @internal 1105 * @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup. 1106 */ 1107 inv_error_t inv_set_dead_zone(void) 1108 { 1109 unsigned char reg; 1110 inv_error_t result; 1111 extern struct control_params cntrl_params; 1112 1113 if (cntrl_params.functions & INV_DEAD_ZONE) { 1114 reg = 0x08; 1115 } else { 1116 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 1117 defined CONFIG_MPU_SENSORS_MPU6050B1 1118 reg = 0; 1119 #else 1120 if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { 1121 reg = 0x2; 1122 } else { 1123 reg = 0; 1124 } 1125 #endif 1126 } 1127 1128 result = inv_set_mpu_memory(KEY_D_0_163, 1, ®); 1129 if (result) { 1130 LOG_RESULT_LOCATION(result); 1131 return result; 1132 } 1133 return result; 1134 } 1135 1136 /** 1137 * @brief inv_set_bias_update is used to register which algorithms will be 1138 * used to automatically reset the gyroscope bias. 1139 * The engine INV_BIAS_UPDATE must be enabled for these algorithms to 1140 * run. 1141 * 1142 * @pre inv_dmp_open() 1143 * @ifnot MPL_MF 1144 * or inv_open_low_power_pedometer() 1145 * or inv_eis_open_dmp() 1146 * @endif 1147 * and inv_dmp_start() 1148 * must <b>NOT</b> have been called. 1149 * 1150 * @param function A function or bitwise OR of functions that determine 1151 * how the gyroscope bias will be automatically updated. 1152 * Functions include: 1153 * - INV_NONE or 0, 1154 * - INV_BIAS_FROM_NO_MOTION, 1155 * - INV_BIAS_FROM_GRAVITY, 1156 * - INV_BIAS_FROM_TEMPERATURE, 1157 \ifnot UMPL 1158 * - INV_BIAS_FROM_LPF, 1159 * - INV_MAG_BIAS_FROM_MOTION, 1160 * - INV_MAG_BIAS_FROM_GYRO, 1161 * - INV_ALL. 1162 * \endif 1163 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1164 */ 1165 inv_error_t inv_set_bias_update(unsigned short function) 1166 { 1167 INVENSENSE_FUNC_START; 1168 unsigned char regs[4]; 1169 long tmp[3] = { 0, 0, 0 }; 1170 inv_error_t result = INV_SUCCESS; 1171 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1172 1173 if (inv_get_state() != INV_STATE_DMP_OPENED) 1174 return INV_ERROR_SM_IMPROPER_STATE; 1175 1176 /* do not allow progressive no motion bias tracker to run - 1177 it's not fully debugged */ 1178 function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround 1179 MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n"); 1180 1181 // You must use EnableFastNoMotion() to get this feature 1182 function &= ~INV_BIAS_FROM_FAST_NO_MOTION; 1183 1184 // You must use DisableFastNoMotion() to turn this feature off 1185 if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) 1186 function |= INV_BIAS_FROM_FAST_NO_MOTION; 1187 1188 /*--- remove magnetic components from bias tracking 1189 if there is no compass ---*/ 1190 if (!inv_compass_present()) { 1191 function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION); 1192 } else { 1193 function &= ~(INV_BIAS_FROM_LPF); 1194 } 1195 1196 // Enable function sets bias from no motion 1197 inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION); 1198 1199 if (function & INV_BIAS_FROM_NO_MOTION) { 1200 inv_enable_bias_no_motion(); 1201 } else { 1202 inv_disable_bias_no_motion(); 1203 } 1204 1205 if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { 1206 regs[0] = DINA80 + 2; 1207 regs[1] = DINA2D; 1208 regs[2] = DINA55; 1209 regs[3] = DINA7D; 1210 } else { 1211 regs[0] = DINA80 + 7; 1212 regs[1] = DINA2D; 1213 regs[2] = DINA35; 1214 regs[3] = DINA3D; 1215 } 1216 result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs); 1217 if (result) { 1218 LOG_RESULT_LOCATION(result); 1219 return result; 1220 } 1221 result = inv_set_dead_zone(); 1222 if (result) { 1223 LOG_RESULT_LOCATION(result); 1224 return result; 1225 } 1226 1227 if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) && 1228 !inv_compass_present()) { 1229 result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION); 1230 if (result) { 1231 LOG_RESULT_LOCATION(result); 1232 return result; 1233 } 1234 } else { 1235 result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW); 1236 if (result) { 1237 LOG_RESULT_LOCATION(result); 1238 return result; 1239 } 1240 } 1241 1242 inv_obj.factory_temp_comp = 0; // FIXME, workaround 1243 if ((mldl_cfg->offset_tc[0] != 0) || 1244 (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) { 1245 inv_obj.factory_temp_comp = 1; 1246 } 1247 1248 if (inv_obj.factory_temp_comp == 0) { 1249 if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) { 1250 result = inv_set_gyro_temp_slope(inv_obj.temp_slope); 1251 if (result) { 1252 LOG_RESULT_LOCATION(result); 1253 return result; 1254 } 1255 } else { 1256 result = inv_set_gyro_temp_slope(tmp); 1257 if (result) { 1258 LOG_RESULT_LOCATION(result); 1259 return result; 1260 } 1261 } 1262 } else { 1263 inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE; 1264 MPL_LOGV("factory temperature compensation coefficients available - " 1265 "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n"); 1266 } 1267 1268 /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on 1269 compass and accel data, is to have accelerometer data delivered in the 1270 FIFO ----*/ 1271 if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) 1272 && inv_compass_present()) 1273 || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) 1274 || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) { 1275 inv_send_accel(INV_ALL, INV_32_BIT); 1276 inv_send_gyro(INV_ALL, INV_32_BIT); 1277 } 1278 1279 return result; 1280 } 1281 1282 /** 1283 * @brief inv_get_motion_state is used to determine if the device is in 1284 * a 'motion' or 'no motion' state. 1285 * inv_get_motion_state returns INV_MOTION of the device is moving, 1286 * or INV_NO_MOTION if the device is not moving. 1287 * 1288 * @pre inv_dmp_open() 1289 * @ifnot MPL_MF 1290 * or inv_open_low_power_pedometer() 1291 * or inv_eis_open_dmp() 1292 * @endif 1293 * and inv_dmp_start() 1294 * must have been called. 1295 * 1296 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1297 */ 1298 int inv_get_motion_state(void) 1299 { 1300 INVENSENSE_FUNC_START; 1301 return inv_obj.motion_state; 1302 } 1303 1304 /** 1305 * @brief inv_set_no_motion_thresh is used to set the threshold for 1306 * detecting INV_NO_MOTION 1307 * 1308 * @pre inv_dmp_open() 1309 * @ifnot MPL_MF 1310 * or inv_open_low_power_pedometer() 1311 * or inv_eis_open_dmp() 1312 * @endif 1313 * must have been called. 1314 * 1315 * @param thresh A threshold scaled in degrees per second. 1316 * 1317 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1318 */ 1319 inv_error_t inv_set_no_motion_thresh(float thresh) 1320 { 1321 inv_error_t result = INV_SUCCESS; 1322 unsigned char regs[4] = { 0 }; 1323 long tmp; 1324 INVENSENSE_FUNC_START; 1325 1326 tmp = (long)(thresh * thresh * 2.045f); 1327 if (tmp < 0) { 1328 return INV_ERROR; 1329 } else if (tmp > 8180000L) { 1330 return INV_ERROR; 1331 } 1332 inv_obj.no_motion_threshold = tmp; 1333 1334 regs[0] = (unsigned char)(tmp >> 24); 1335 regs[1] = (unsigned char)((tmp >> 16) & 0xff); 1336 regs[2] = (unsigned char)((tmp >> 8) & 0xff); 1337 regs[3] = (unsigned char)(tmp & 0xff); 1338 1339 result = inv_set_mpu_memory(KEY_D_1_108, 4, regs); 1340 if (result) { 1341 LOG_RESULT_LOCATION(result); 1342 return result; 1343 } 1344 result = inv_reset_motion(); 1345 return result; 1346 } 1347 1348 /** 1349 * @brief inv_set_no_motion_threshAccel is used to set the threshold for 1350 * detecting INV_NO_MOTION with accelerometers when Gyros have 1351 * been turned off 1352 * 1353 * @pre inv_dmp_open() 1354 * @ifnot MPL_MF 1355 * or inv_open_low_power_pedometer() 1356 * or inv_eis_open_dmp() 1357 * @endif 1358 * must have been called. 1359 * 1360 * @param thresh A threshold in g's scaled by 2^32 1361 * 1362 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1363 */ 1364 inv_error_t inv_set_no_motion_threshAccel(long thresh) 1365 { 1366 INVENSENSE_FUNC_START; 1367 1368 inv_obj.no_motion_accel_threshold = thresh; 1369 1370 return INV_SUCCESS; 1371 } 1372 1373 /** 1374 * @brief inv_set_no_motion_time is used to set the time required for 1375 * detecting INV_NO_MOTION 1376 * 1377 * @pre inv_dmp_open() 1378 * @ifnot MPL_MF 1379 * or inv_open_low_power_pedometer() 1380 * or inv_eis_open_dmp() 1381 * @endif 1382 * must have been called. 1383 * 1384 * @param time A time in seconds. 1385 * 1386 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1387 */ 1388 inv_error_t inv_set_no_motion_time(float time) 1389 { 1390 inv_error_t result = INV_SUCCESS; 1391 unsigned char regs[2] = { 0 }; 1392 long tmp; 1393 1394 INVENSENSE_FUNC_START; 1395 1396 tmp = (long)(time * 200); 1397 if (tmp < 0) { 1398 return INV_ERROR; 1399 } else if (tmp > 65535L) { 1400 return INV_ERROR; 1401 } 1402 inv_obj.motion_duration = (unsigned short)tmp; 1403 1404 regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); 1405 regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); 1406 result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); 1407 if (result) { 1408 LOG_RESULT_LOCATION(result); 1409 return result; 1410 } 1411 result = inv_reset_motion(); 1412 return result; 1413 } 1414 1415 /** 1416 * @brief inv_get_version is used to get the ML version. 1417 * 1418 * @pre inv_get_version can be called at any time. 1419 * 1420 * @param version inv_get_version writes the ML version 1421 * string pointer to version. 1422 * 1423 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1424 */ 1425 inv_error_t inv_get_version(unsigned char **version) 1426 { 1427 INVENSENSE_FUNC_START; 1428 1429 *version = (unsigned char *)mlVer; //fixme we are wiping const 1430 1431 return INV_SUCCESS; 1432 } 1433 1434 /** 1435 * @brief Check for the presence of the gyro sensor. 1436 * 1437 * This is not a physical check but a logical check and the value can change 1438 * dynamically based on calls to inv_set_mpu_sensors(). 1439 * 1440 * @return TRUE if the gyro is enabled FALSE otherwise. 1441 */ 1442 int inv_get_gyro_present(void) 1443 { 1444 return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO | 1445 INV_Z_GYRO); 1446 } 1447 1448 static unsigned short inv_row_2_scale(const signed char *row) 1449 { 1450 unsigned short b; 1451 1452 if (row[0] > 0) 1453 b = 0; 1454 else if (row[0] < 0) 1455 b = 4; 1456 else if (row[1] > 0) 1457 b = 1; 1458 else if (row[1] < 0) 1459 b = 5; 1460 else if (row[2] > 0) 1461 b = 2; 1462 else if (row[2] < 0) 1463 b = 6; 1464 else 1465 b = 7; // error 1466 return b; 1467 } 1468 1469 unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) 1470 { 1471 unsigned short scalar; 1472 /* 1473 XYZ 010_001_000 Identity Matrix 1474 XZY 001_010_000 1475 YXZ 010_000_001 1476 YZX 000_010_001 1477 ZXY 001_000_010 1478 ZYX 000_001_010 1479 */ 1480 1481 scalar = inv_row_2_scale(mtx); 1482 scalar |= inv_row_2_scale(mtx + 3) << 3; 1483 scalar |= inv_row_2_scale(mtx + 6) << 6; 1484 1485 return scalar; 1486 } 1487 1488 /* Setups up the Freescale 16-bit accel for Sensor Fusion 1489 * @param[in] orient A scalar representation of the orientation 1490 * that describes how to go from the Chip Orientation 1491 * to the Board Orientation often times called the Body Orientation in Invensense Documentation. 1492 * inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. 1493 */ 1494 inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient) 1495 { 1496 unsigned char rr[3]; 1497 inv_error_t result; 1498 1499 orient = orient & 0xdb; 1500 switch (orient) { 1501 default: 1502 // Typically 0x88 1503 rr[0] = DINACC; 1504 rr[1] = DINACF; 1505 rr[2] = DINA0E; 1506 break; 1507 case 0x50: 1508 rr[0] = DINACE; 1509 rr[1] = DINA0E; 1510 rr[2] = DINACD; 1511 break; 1512 case 0x81: 1513 rr[0] = DINACE; 1514 rr[1] = DINACB; 1515 rr[2] = DINA0E; 1516 break; 1517 case 0x11: 1518 rr[0] = DINACC; 1519 rr[1] = DINA0E; 1520 rr[2] = DINACB; 1521 break; 1522 case 0x42: 1523 rr[0] = DINA0A; 1524 rr[1] = DINACF; 1525 rr[2] = DINACB; 1526 break; 1527 case 0x0a: 1528 rr[0] = DINA0A; 1529 rr[1] = DINACB; 1530 rr[2] = DINACD; 1531 break; 1532 } 1533 result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr); 1534 return result; 1535 } 1536 1537 /* Setups up the Freescale 8-bit accel for Sensor Fusion 1538 * @param[in] orient A scalar representation of the orientation 1539 * that describes how to go from the Chip Orientation 1540 * to the Board Orientation often times called the Body Orientation in Invensense Documentation. 1541 * inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. 1542 */ 1543 inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient) 1544 { 1545 unsigned char regs[27]; 1546 inv_error_t result; 1547 uint_fast8_t kk; 1548 1549 orient = orient & 0xdb; 1550 kk = 0; 1551 1552 regs[kk++] = DINAC3; 1553 regs[kk++] = DINA90 + 14; 1554 regs[kk++] = DINAA0 + 9; 1555 regs[kk++] = DINA3E; 1556 regs[kk++] = DINA5E; 1557 regs[kk++] = DINA7E; 1558 1559 regs[kk++] = DINAC2; 1560 regs[kk++] = DINAA0 + 9; 1561 regs[kk++] = DINA90 + 9; 1562 regs[kk++] = DINAF8 + 2; 1563 1564 switch (orient) { 1565 default: 1566 // Typically 0x88 1567 regs[kk++] = DINACB; 1568 1569 regs[kk++] = DINA54; 1570 regs[kk++] = DINA50; 1571 regs[kk++] = DINA50; 1572 regs[kk++] = DINA50; 1573 regs[kk++] = DINA50; 1574 regs[kk++] = DINA50; 1575 regs[kk++] = DINA50; 1576 regs[kk++] = DINA50; 1577 1578 regs[kk++] = DINACD; 1579 break; 1580 case 0x50: 1581 regs[kk++] = DINACB; 1582 1583 regs[kk++] = DINACF; 1584 1585 regs[kk++] = DINA7C; 1586 regs[kk++] = DINA78; 1587 regs[kk++] = DINA78; 1588 regs[kk++] = DINA78; 1589 regs[kk++] = DINA78; 1590 regs[kk++] = DINA78; 1591 regs[kk++] = DINA78; 1592 regs[kk++] = DINA78; 1593 break; 1594 case 0x81: 1595 regs[kk++] = DINA2C; 1596 regs[kk++] = DINA28; 1597 regs[kk++] = DINA28; 1598 regs[kk++] = DINA28; 1599 regs[kk++] = DINA28; 1600 regs[kk++] = DINA28; 1601 regs[kk++] = DINA28; 1602 regs[kk++] = DINA28; 1603 1604 regs[kk++] = DINACD; 1605 1606 regs[kk++] = DINACB; 1607 break; 1608 case 0x11: 1609 regs[kk++] = DINA2C; 1610 regs[kk++] = DINA28; 1611 regs[kk++] = DINA28; 1612 regs[kk++] = DINA28; 1613 regs[kk++] = DINA28; 1614 regs[kk++] = DINA28; 1615 regs[kk++] = DINA28; 1616 regs[kk++] = DINA28; 1617 regs[kk++] = DINACB; 1618 regs[kk++] = DINACF; 1619 break; 1620 case 0x42: 1621 regs[kk++] = DINACF; 1622 regs[kk++] = DINACD; 1623 1624 regs[kk++] = DINA7C; 1625 regs[kk++] = DINA78; 1626 regs[kk++] = DINA78; 1627 regs[kk++] = DINA78; 1628 regs[kk++] = DINA78; 1629 regs[kk++] = DINA78; 1630 regs[kk++] = DINA78; 1631 regs[kk++] = DINA78; 1632 break; 1633 case 0x0a: 1634 regs[kk++] = DINACD; 1635 1636 regs[kk++] = DINA54; 1637 regs[kk++] = DINA50; 1638 regs[kk++] = DINA50; 1639 regs[kk++] = DINA50; 1640 regs[kk++] = DINA50; 1641 regs[kk++] = DINA50; 1642 regs[kk++] = DINA50; 1643 regs[kk++] = DINA50; 1644 1645 regs[kk++] = DINACF; 1646 break; 1647 } 1648 1649 regs[kk++] = DINA90 + 7; 1650 regs[kk++] = DINAF8 + 3; 1651 regs[kk++] = DINAA0 + 9; 1652 regs[kk++] = DINA0E; 1653 regs[kk++] = DINA0E; 1654 regs[kk++] = DINA0E; 1655 1656 regs[kk++] = DINAF8 + 1; // filler 1657 1658 result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs); 1659 if (result) { 1660 LOG_RESULT_LOCATION(result); 1661 return result; 1662 } 1663 1664 return result; 1665 } 1666 1667 /** 1668 * Controlls each sensor and each axis when the motion processing unit is 1669 * running. When it is not running, simply records the state for later. 1670 * 1671 * NOTE: In this version only full sensors controll is allowed. Independent 1672 * axis control will return an error. 1673 * 1674 * @param sensors Bit field of each axis desired to be turned on or off 1675 * 1676 * @return INV_SUCCESS or non-zero error code 1677 */ 1678 inv_error_t inv_set_mpu_sensors(unsigned long sensors) 1679 { 1680 INVENSENSE_FUNC_START; 1681 unsigned char state = inv_get_state(); 1682 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1683 inv_error_t result = INV_SUCCESS; 1684 unsigned short fifoRate; 1685 1686 if (state < INV_STATE_DMP_OPENED) 1687 return INV_ERROR_SM_IMPROPER_STATE; 1688 1689 if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) && 1690 ((sensors & INV_THREE_AXIS_ACCEL) != 0)) { 1691 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 1692 } 1693 if (((sensors & INV_THREE_AXIS_ACCEL) != 0) && 1694 (mldl_cfg->pdata->accel.get_slave_descr == 0)) { 1695 return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; 1696 } 1697 1698 if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) && 1699 ((sensors & INV_THREE_AXIS_COMPASS) != 0)) { 1700 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 1701 } 1702 if (((sensors & INV_THREE_AXIS_COMPASS) != 0) && 1703 (mldl_cfg->pdata->compass.get_slave_descr == 0)) { 1704 return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; 1705 } 1706 1707 if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) && 1708 ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) { 1709 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 1710 } 1711 if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) && 1712 (mldl_cfg->pdata->pressure.get_slave_descr == 0)) { 1713 return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; 1714 } 1715 1716 /* DMP was off, and is turning on */ 1717 if (sensors & INV_DMP_PROCESSOR && 1718 !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { 1719 struct ext_slave_config config; 1720 long odr; 1721 config.key = MPU_SLAVE_CONFIG_ODR_RESUME; 1722 config.len = sizeof(long); 1723 config.apply = (state == INV_STATE_DMP_STARTED); 1724 config.data = &odr; 1725 1726 odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000); 1727 result = inv_mpu_config_accel(mldl_cfg, 1728 inv_get_serial_handle(), 1729 inv_get_serial_handle(), &config); 1730 if (result) { 1731 LOG_RESULT_LOCATION(result); 1732 return result; 1733 } 1734 1735 config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; 1736 odr = MPU_SLAVE_IRQ_TYPE_NONE; 1737 result = inv_mpu_config_accel(mldl_cfg, 1738 inv_get_serial_handle(), 1739 inv_get_serial_handle(), &config); 1740 if (result) { 1741 LOG_RESULT_LOCATION(result); 1742 return result; 1743 } 1744 inv_init_fifo_hardare(); 1745 } 1746 1747 if (inv_obj.mode_change_func) { 1748 result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors); 1749 if (result) { 1750 LOG_RESULT_LOCATION(result); 1751 return result; 1752 } 1753 } 1754 1755 /* Get the fifo rate before changing sensors so if we need to match it */ 1756 fifoRate = inv_get_fifo_rate(); 1757 mldl_cfg->requested_sensors = sensors; 1758 1759 /* inv_dmp_start will turn the sensors on */ 1760 if (state == INV_STATE_DMP_STARTED) { 1761 result = inv_dl_start(sensors); 1762 if (result) { 1763 LOG_RESULT_LOCATION(result); 1764 return result; 1765 } 1766 result = inv_reset_motion(); 1767 if (result) { 1768 LOG_RESULT_LOCATION(result); 1769 return result; 1770 } 1771 result = inv_dl_stop(~sensors); 1772 if (result) { 1773 LOG_RESULT_LOCATION(result); 1774 return result; 1775 } 1776 } 1777 1778 inv_set_fifo_rate(fifoRate); 1779 1780 if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) { 1781 struct ext_slave_config config; 1782 long data; 1783 1784 config.len = sizeof(long); 1785 config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; 1786 config.apply = (state == INV_STATE_DMP_STARTED); 1787 config.data = &data; 1788 data = MPU_SLAVE_IRQ_TYPE_DATA_READY; 1789 result = inv_mpu_config_accel(mldl_cfg, 1790 inv_get_serial_handle(), 1791 inv_get_serial_handle(), &config); 1792 if (result) { 1793 LOG_RESULT_LOCATION(result); 1794 return result; 1795 } 1796 } 1797 1798 return result; 1799 } 1800 1801 void inv_set_mode_change(inv_error_t(*mode_change_func) 1802 (unsigned long, unsigned long)) 1803 { 1804 inv_obj.mode_change_func = mode_change_func; 1805 } 1806 1807 /** 1808 * Mantis setup 1809 */ 1810 #ifdef CONFIG_MPU_SENSORS_MPU6050B1 1811 inv_error_t inv_set_mpu_6050_config() 1812 { 1813 long temp; 1814 inv_error_t result; 1815 unsigned char big8[4]; 1816 unsigned char atc[4]; 1817 long s[3], s2[3]; 1818 int kk; 1819 struct mldl_cfg *mldlCfg = inv_get_dl_config(); 1820 1821 result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 1822 0x0d, 4, atc); 1823 if (result) { 1824 LOG_RESULT_LOCATION(result); 1825 return result; 1826 } 1827 1828 temp = atc[3] & 0x3f; 1829 if (temp >= 32) 1830 temp = temp - 64; 1831 temp = (temp << 21) | 0x100000; 1832 temp += (1L << 29); 1833 temp = -temp; 1834 result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8)); 1835 if (result) { 1836 LOG_RESULT_LOCATION(result); 1837 return result; 1838 } 1839 1840 for (kk = 0; kk < 3; ++kk) { 1841 s[kk] = atc[kk] & 0x3f; 1842 if (s[kk] > 32) 1843 s[kk] = s[kk] - 64; 1844 s[kk] *= 2516582L; 1845 } 1846 1847 for (kk = 0; kk < 3; ++kk) { 1848 s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] + 1849 mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] + 1850 mldlCfg->pdata->orientation[kk * 3 + 2] * s[2]; 1851 } 1852 result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8)); 1853 if (result) { 1854 LOG_RESULT_LOCATION(result); 1855 return result; 1856 } 1857 result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8)); 1858 if (result) { 1859 LOG_RESULT_LOCATION(result); 1860 return result; 1861 } 1862 result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8)); 1863 if (result) { 1864 LOG_RESULT_LOCATION(result); 1865 return result; 1866 } 1867 1868 return result; 1869 } 1870 #endif 1871 1872 /** 1873 * @} 1874 */ 1875
