/*
$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;
}
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;
}
/**
* @}
*/