328 lines
9.2 KiB
C
328 lines
9.2 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: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
|
|
*
|
|
*******************************************************************************/
|
|
|
|
/**
|
|
* @defgroup MLFIFO_HW
|
|
* @brief Motion Library - FIFO HW Driver.
|
|
* Provides facilities to interact with the FIFO.
|
|
*
|
|
* @{
|
|
* @file mlFIFOHW.c
|
|
* @brief The Motion Library Fifo Hardware Layer.
|
|
*
|
|
*/
|
|
|
|
#include <string.h>
|
|
|
|
#include "mpu.h"
|
|
#if defined CONFIG_MPU_SENSORS_MPU6050A2
|
|
# include "mpu6050a2.h"
|
|
#elif defined CONFIG_MPU_SENSORS_MPU6050B1
|
|
# include "mpu6050b1.h"
|
|
#elif defined CONFIG_MPU_SENSORS_MPU3050
|
|
# include "mpu3050.h"
|
|
#else
|
|
#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
|
|
#endif
|
|
#include "mlFIFOHW.h"
|
|
#include "ml.h"
|
|
#include "mldl.h"
|
|
#include "mldl_cfg.h"
|
|
|
|
#include "mlsl.h"
|
|
|
|
#include "log.h"
|
|
#undef MPL_LOG_TAG
|
|
#define MPL_LOG_TAG "MPL-fifo"
|
|
|
|
/*
|
|
Defines
|
|
*/
|
|
|
|
#define _fifoDebug(x) //{x}
|
|
|
|
/*
|
|
Typedefs
|
|
*/
|
|
|
|
struct fifo_hw_obj {
|
|
short fifoCount;
|
|
inv_error_t fifoError;
|
|
unsigned char fifoOverflow;
|
|
unsigned char fifoResetOnOverflow;
|
|
};
|
|
|
|
/*
|
|
Global variables
|
|
*/
|
|
const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
|
|
|
|
/*
|
|
Static variables
|
|
*/
|
|
static struct fifo_hw_obj fifo_objHW;
|
|
|
|
/*
|
|
Definitions
|
|
*/
|
|
|
|
/**
|
|
* @brief Initializes the internal FIFO data structure.
|
|
*/
|
|
void inv_init_fifo_hardare(void)
|
|
{
|
|
memset(&fifo_objHW, 0, sizeof(fifo_objHW));
|
|
fifo_objHW.fifoResetOnOverflow = TRUE;
|
|
}
|
|
|
|
/**
|
|
* @internal
|
|
* @brief used to get the FIFO data.
|
|
* @param length
|
|
* Number of bytes to read from the FIFO.
|
|
* @param buffer
|
|
* the bytes of FIFO data.
|
|
* Note that this buffer <b>must</b> be large enough
|
|
* to store and additional trailing FIFO footer when
|
|
* expected. The callers must make sure enough space
|
|
* is allocated.
|
|
* @return number of valid bytes of data.
|
|
**/
|
|
uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
|
|
{
|
|
INVENSENSE_FUNC_START;
|
|
inv_error_t result;
|
|
uint_fast16_t inFifo;
|
|
uint_fast16_t toRead;
|
|
int_fast8_t kk;
|
|
|
|
toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
|
|
/*---- make sure length is correct ----*/
|
|
if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
|
|
fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
|
|
return 0;
|
|
}
|
|
|
|
result = inv_get_fifo_length(&inFifo);
|
|
if (INV_SUCCESS != result) {
|
|
fifo_objHW.fifoError = result;
|
|
return 0;
|
|
}
|
|
// fifo_objHW.fifoCount is the footer size left in the buffer, or
|
|
// 0 if this is the first time reading the fifo since it was reset
|
|
if (inFifo < length + fifo_objHW.fifoCount) {
|
|
fifo_objHW.fifoError = INV_SUCCESS;
|
|
return 0;
|
|
}
|
|
// if a trailing fifo count is expected - start storing data 2 bytes before
|
|
result =
|
|
inv_read_fifo(fifo_objHW.fifoCount >
|
|
0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
|
|
if (INV_SUCCESS != result) {
|
|
fifo_objHW.fifoError = result;
|
|
return 0;
|
|
}
|
|
// Make sure the fifo didn't overflow before or during the read
|
|
result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
|
|
MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
|
|
if (INV_SUCCESS != result) {
|
|
fifo_objHW.fifoError = result;
|
|
return 0;
|
|
}
|
|
|
|
if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
|
|
MPL_LOGV("Resetting Fifo : Overflow\n");
|
|
inv_reset_fifo();
|
|
fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
|
|
return 0;
|
|
}
|
|
|
|
/* Check the Footer value to give us a chance at making sure data
|
|
* didn't get corrupted */
|
|
for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
|
|
if (buffer[kk] != gFifoFooter[kk]) {
|
|
MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
|
|
buffer[0], buffer[1]);
|
|
_fifoDebug(char out[200];
|
|
MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
|
|
sprintf(out, "0x");
|
|
for (kk = 0; kk < (int)toRead; kk++) {
|
|
sprintf(out, "%s%02X", out, buffer[kk]);}
|
|
MPL_LOGW("%s\n", out);)
|
|
inv_reset_fifo();
|
|
fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
if (fifo_objHW.fifoCount == 0) {
|
|
fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
|
|
}
|
|
|
|
return length - FIFO_FOOTER_SIZE;
|
|
}
|
|
|
|
/**
|
|
* @brief Used to query the status of the FIFO.
|
|
* @return INV_SUCCESS if the fifo is OK. An error code otherwise.
|
|
**/
|
|
inv_error_t inv_get_fifo_status(void)
|
|
{
|
|
inv_error_t fifoError = fifo_objHW.fifoError;
|
|
fifo_objHW.fifoError = 0;
|
|
return fifoError;
|
|
}
|
|
|
|
/**
|
|
* @internal
|
|
* @brief Get the length from the fifo
|
|
*
|
|
* @param[out] len amount of data currently stored in the fifo.
|
|
*
|
|
* @return INV_SUCCESS or non-zero error code.
|
|
**/
|
|
inv_error_t inv_get_fifo_length(uint_fast16_t * len)
|
|
{
|
|
INVENSENSE_FUNC_START;
|
|
unsigned char fifoBuf[2];
|
|
inv_error_t result;
|
|
|
|
if (NULL == len)
|
|
return INV_ERROR_INVALID_PARAMETER;
|
|
|
|
/*---- read the 2 'count' registers and
|
|
burst read the data from the FIFO ----*/
|
|
result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
|
|
MPUREG_FIFO_COUNTH, 2, fifoBuf);
|
|
if (INV_SUCCESS != result) {
|
|
MPL_LOGE("ReadBurst failed %d\n", result);
|
|
inv_reset_fifo();
|
|
fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
|
|
*len = 0;
|
|
return result;
|
|
}
|
|
|
|
*len = (uint_fast16_t) (fifoBuf[0] << 8);
|
|
*len += (uint_fast16_t) (fifoBuf[1]);
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* @brief inv_get_fifo_count is used to get the number of bytes left in the FIFO.
|
|
* This function returns the stored value and does not access the hardware.
|
|
* See inv_get_fifo_length().
|
|
* @return the number of bytes left in the FIFO
|
|
**/
|
|
short inv_get_fifo_count(void)
|
|
{
|
|
return fifo_objHW.fifoCount;
|
|
}
|
|
|
|
/**
|
|
* @internal
|
|
* @brief Read data from the fifo
|
|
*
|
|
* @param[out] data Location to store the date read from the fifo
|
|
* @param[in] len Amount of data to read out of the fifo
|
|
*
|
|
* @return INV_SUCCESS or non-zero error code
|
|
**/
|
|
inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
|
|
{
|
|
INVENSENSE_FUNC_START;
|
|
inv_error_t result;
|
|
result = inv_serial_read_fifo(inv_get_serial_handle(),
|
|
inv_get_mpu_slave_addr(),
|
|
(unsigned short)len, data);
|
|
if (INV_SUCCESS != result) {
|
|
MPL_LOGE("inv_serial_readBurst failed %d\n", result);
|
|
inv_reset_fifo();
|
|
fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
|
|
return result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* @brief Clears the FIFO status and its content.
|
|
* @note Halt the DMP writing into the FIFO for the time
|
|
* needed to reset the FIFO.
|
|
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
|
|
*/
|
|
inv_error_t inv_reset_fifo(void)
|
|
{
|
|
INVENSENSE_FUNC_START;
|
|
int len = FIFO_HW_SIZE;
|
|
unsigned char fifoBuf[2];
|
|
unsigned char tries = 0;
|
|
unsigned char userCtrlReg;
|
|
inv_error_t result;
|
|
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
|
|
|
|
fifo_objHW.fifoCount = 0;
|
|
if (mldl_cfg->gyro_is_suspended)
|
|
return INV_SUCCESS;
|
|
|
|
result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
|
|
MPUREG_USER_CTRL, 1, &userCtrlReg);
|
|
if (result) {
|
|
LOG_RESULT_LOCATION(result);
|
|
return result;
|
|
}
|
|
|
|
while (len != 0 && tries < 6) {
|
|
result =
|
|
inv_serial_single_write(inv_get_serial_handle(),
|
|
inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
|
|
((userCtrlReg & (~BIT_FIFO_EN)) |
|
|
BIT_FIFO_RST));
|
|
if (result) {
|
|
LOG_RESULT_LOCATION(result);
|
|
return result;
|
|
}
|
|
result =
|
|
inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
|
|
MPUREG_FIFO_COUNTH, 2, fifoBuf);
|
|
if (result) {
|
|
LOG_RESULT_LOCATION(result);
|
|
return result;
|
|
}
|
|
len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
|
|
tries++;
|
|
}
|
|
|
|
result =
|
|
inv_serial_single_write(inv_get_serial_handle(),
|
|
inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
|
|
userCtrlReg);
|
|
if (result) {
|
|
LOG_RESULT_LOCATION(result);
|
|
return result;
|
|
}
|
|
|
|
return INV_SUCCESS;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
**/
|