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