diff options
Diffstat (limited to 'invensense/mlsdk/mllite/mldmp.c')
-rw-r--r-- | invensense/mlsdk/mllite/mldmp.c | 284 |
1 files changed, 284 insertions, 0 deletions
diff --git a/invensense/mlsdk/mllite/mldmp.c b/invensense/mlsdk/mllite/mldmp.c new file mode 100644 index 0000000..d16bdaf --- /dev/null +++ b/invensense/mlsdk/mllite/mldmp.c @@ -0,0 +1,284 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mldmp.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +/** + * @addtogroup MLDMP + * + * @{ + * @file mldmp.c + * @brief Shared functions between all the different DMP versions +**/ + +#include <stdio.h> + +#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 "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(); + + 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(); + requested_sensors = INV_THREE_AXIS_GYRO; + if (mldl_cfg->accel && mldl_cfg->accel->resume) + requested_sensors |= INV_THREE_AXIS_ACCEL; + + if (mldl_cfg->compass && mldl_cfg->compass->resume) + requested_sensors |= INV_THREE_AXIS_COMPASS; + + if (mldl_cfg->pressure && mldl_cfg->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; + } + if (NULL != mldl_cfg->accel){ + result = inv_apply_endian_accel(); + } + + 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; + + 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()->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; + } + + 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; + + 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_dl_stop(INV_ALL_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; + + 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; +} + +/** + * @} + */ |