284 lines
7.1 KiB
C
284 lines
7.1 KiB
C
/*
|
|
$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;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
*/
|