/* $License: Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. $ */ /****************************************************************************** * * $Id: mldmp.c 5773 2011-07-14 22:28:32Z nroyer $ * *****************************************************************************/ /** * @addtogroup MLDMP * * @{ * @file mldmp.c * @brief Shared functions between all the different DMP versions **/ #include #include "mltypes.h" #include "mlinclude.h" #include "mltypes.h" #include "ml.h" #include "mldl_cfg.h" #include "mldl.h" #include "compass.h" #include "mlSetGyroBias.h" #include "mlsl.h" #include "mlFIFO.h" #include "mldmp.h" #include "mlstates.h" #include "dmpDefault.h" #include "mlFIFOHW.h" #include "mlsupervisor.h" #include "compass_supervisor.h" #include "key0_96.h" #include "log.h" #undef MPL_LOG_TAG #define MPL_LOG_TAG "MPL-dmp" /** * @brief Open the default motion sensor engine. * This function is used to open the default MPL engine, * featuring, for example, sensor fusion (6 axes and 9 axes), * sensor calibration, accelerometer data byte swapping, among * others. * Compare with the other provided engines. * * @pre inv_serial_start() must have been called to instantiate the serial * communication. * * Example: * @code * result = inv_dmp_open( ); * if (INV_SUCCESS != result) { * // Handle the error case * } * @endcode * * @return Zero on success; Error Code on any failure. * */ inv_error_t inv_dmp_open(void) { INVENSENSE_FUNC_START; inv_error_t result; unsigned char state = inv_get_state(); struct mldl_cfg *mldl_cfg; unsigned long requested_sensors; /************************************************************* * Common operations before calling DMPOpen ************************************************************/ if (state == INV_STATE_DMP_OPENED) return INV_SUCCESS; if (state == INV_STATE_DMP_STARTED) { return inv_dmp_stop(); } result = inv_state_transition(INV_STATE_DMP_OPENED); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_dl_open(inv_get_serial_handle()); if (result) { LOG_RESULT_LOCATION(result); return result; } #ifdef ML_USE_DMP_SIM do { void setup_univ(); setup_univ(); /* hijack the read and write paths and re-direct them to the simulator */ } while (0); #endif result = inv_setup_dmp(); if (result) { LOG_RESULT_LOCATION(result); return result; } // Init vars. inv_init_ml(); inv_init_ml_cb(); result = inv_init_fifo_param(); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_enable_set_bias(); if (result) { LOG_RESULT_LOCATION(result); return result; } inv_init_fifo_hardare(); mldl_cfg = inv_get_dl_config(); if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) { result = inv_read_compass_asa(inv_obj.mag->asa); if (result == INV_SUCCESS) { MPL_LOGI("Successfully read compass asa\n"); } } requested_sensors = INV_THREE_AXIS_GYRO; if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] && mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->resume) requested_sensors |= INV_THREE_AXIS_ACCEL; if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] && mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->resume) requested_sensors |= INV_THREE_AXIS_COMPASS; if (mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE] && mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE]->resume) requested_sensors |= INV_THREE_AXIS_PRESSURE; result = inv_init_requested_sensors(requested_sensors); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_apply_calibration(); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_apply_endian_accel(); if (result) { LOG_RESULT_LOCATION(result); return result; } #ifdef UMPL_NINE_AXIS result = inv_enable_compass_supervisor(); if (result) { LOG_RESULT_LOCATION(result); return result; } #endif return result; } /** * @brief Start the DMP. * * @pre inv_dmp_open() must have been called. * * @code * result = inv_dmp_start(); * if (INV_SUCCESS != result) { * // Handle the error case * } * @endcode * * @return INV_SUCCESS if successful, or Non-zero error code otherwise. */ inv_error_t inv_dmp_start(void) { INVENSENSE_FUNC_START; inv_error_t result; if (inv_get_state() == INV_STATE_DMP_STARTED) return INV_SUCCESS; /* Power the gyro before transitioning state so that items that would * normally be cached go directly to the gyro */ if (INV_CACHE_DMP == 0) { unsigned long requested_sensors = inv_get_dl_config()->inv_mpu_cfg->requested_sensors; result = inv_dl_start(INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO); if (result) { LOG_RESULT_LOCATION(result); return result; } inv_init_requested_sensors(requested_sensors); } result = inv_state_transition(INV_STATE_DMP_STARTED); if (result) { LOG_RESULT_LOCATION(result); return result; } inv_init_sensor_fusion_supervisor(); result = inv_dl_start(inv_get_dl_config()->inv_mpu_cfg->requested_sensors); if (result) { LOG_RESULT_LOCATION(result); return result; } /* This is done after the start since it will modify DMP memory, which * will cause a full reset is most cases */ result = inv_reset_motion(); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_register_fifo_rate_process(inv_key_0_96, INV_PRIORITY_KEY_0_96); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; } /** * @brief Stops the DMP and puts it in low power. * * @pre inv_dmp_start() must have been called. * * @return INV_SUCCESS, Non-zero error code otherwise. */ inv_error_t inv_dmp_stop(void) { INVENSENSE_FUNC_START; inv_error_t result; unsigned long requested_sensors; if (inv_get_state() == INV_STATE_DMP_OPENED) return INV_SUCCESS; result = inv_state_transition(INV_STATE_DMP_OPENED); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_unregister_fifo_rate_process(inv_key_0_96); if (result) { LOG_RESULT_LOCATION(result); return result; } requested_sensors = inv_get_dl_config()->inv_mpu_cfg->requested_sensors; result = inv_dl_stop(INV_ALL_SENSORS); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_init_requested_sensors(requested_sensors); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; } /** * @brief Closes the motion sensor engine. * Does not close the serial communication. To do that, * call inv_serial_stop(). * After calling inv_dmp_close() another DMP module can be * loaded in the MPL with the corresponding necessary * intialization and configurations, via any of the * MLDmpXXXOpen functions. * * @pre inv_dmp_open() must have been called. * * @code * result = inv_dmp_close(); * if (INV_SUCCESS != result) { * // Handle the error case * } * @endcode * * @return INV_SUCCESS, Non-zero error code otherwise. */ inv_error_t inv_dmp_close(void) { INVENSENSE_FUNC_START; inv_error_t result; inv_error_t firstError = INV_SUCCESS; if (inv_get_state() <= INV_STATE_DMP_CLOSED) return INV_SUCCESS; #ifdef UMPL_NINE_AXIS result = inv_disable_compass_supervisor(); ERROR_CHECK_FIRST(firstError, result); #endif result = inv_disable_set_bias(); ERROR_CHECK_FIRST(firstError, result); result = inv_dl_stop(INV_ALL_SENSORS); ERROR_CHECK_FIRST(firstError, result); result = inv_close_fifo(); ERROR_CHECK_FIRST(firstError, result); result = inv_dl_close(); ERROR_CHECK_FIRST(firstError, result); result = inv_state_transition(INV_STATE_SERIAL_OPENED); ERROR_CHECK_FIRST(firstError, result); return result; } /** * @} */