1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/******************************************************************************* 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $ 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *******************************************************************************/ 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup MLFIFO_HW 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Motion Library - FIFO HW Driver. 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Provides facilities to interact with the FIFO. 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file mlFIFOHW.c 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The Motion Library Fifo Hardware Layer. 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mpu.h" 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu6050a2.h" 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#elif defined CONFIG_MPU_SENSORS_MPU6050B1 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu6050b1.h" 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#elif defined CONFIG_MPU_SENSORS_MPU3050 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu3050.h" 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFOHW.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-fifo" 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Defines 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define _fifoDebug(x) //{x} 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Typedefs 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct fifo_hw_obj { 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short fifoCount; 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t fifoError; 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char fifoOverflow; 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char fifoResetOnOverflow; 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Global variables 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallconst unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A }; 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Static variables 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic struct fifo_hw_obj fifo_objHW; 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Definitions 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Initializes the internal FIFO data structure. 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_init_fifo_hardare(void) 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&fifo_objHW, 0, sizeof(fifo_objHW)); 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoResetOnOverflow = TRUE; 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief used to get the FIFO data. 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Number of bytes to read from the FIFO. 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the bytes of FIFO data. 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Note that this buffer <b>must</b> be large enough 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * to store and additional trailing FIFO footer when 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * expected. The callers must make sure enough space 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is allocated. 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return number of valid bytes of data. 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgralluint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer) 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t inFifo; 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t toRead; 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t kk; 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount; 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- make sure length is correct ----*/ 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) { 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER; 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_fifo_length(&inFifo); 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = result; 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // fifo_objHW.fifoCount is the footer size left in the buffer, or 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // 0 if this is the first time reading the fifo since it was reset 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inFifo < length + fifo_objHW.fifoCount) { 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = INV_SUCCESS; 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // if a trailing fifo count is expected - start storing data 2 bytes before 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_read_fifo(fifo_objHW.fifoCount > 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead); 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = result; 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure the fifo didn't overflow before or during the read 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow); 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = result; 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) { 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("Resetting Fifo : Overflow\n"); 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_fifo(); 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Check the Footer value to give us a chance at making sure data 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * didn't get corrupted */ 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) { 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (buffer[kk] != gFifoFooter[kk]) { 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n", 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall buffer[0], buffer[1]); 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall _fifoDebug(char out[200]; 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount); 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(out, "0x"); 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < (int)toRead; kk++) { 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sprintf(out, "%s%02X", out, buffer[kk]);} 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGW("%s\n", out);) 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_fifo(); 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER; 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_objHW.fifoCount == 0) { 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoCount = FIFO_FOOTER_SIZE; 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return length - FIFO_FOOTER_SIZE; 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Used to query the status of the FIFO. 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if the fifo is OK. An error code otherwise. 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_fifo_status(void) 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t fifoError = fifo_objHW.fifoError; 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = 0; 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return fifoError; 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the length from the fifo 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] len amount of data currently stored in the fifo. 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code. 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_fifo_length(uint_fast16_t * len) 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char fifoBuf[2]; 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (NULL == len) 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- read the 2 'count' registers and 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall burst read the data from the FIFO ----*/ 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_COUNTH, 2, fifoBuf); 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("ReadBurst failed %d\n", result); 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_fifo(); 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT; 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *len = 0; 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *len = (uint_fast16_t) (fifoBuf[0] << 8); 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *len += (uint_fast16_t) (fifoBuf[1]); 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_get_fifo_count is used to get the number of bytes left in the FIFO. 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This function returns the stored value and does not access the hardware. 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * See inv_get_fifo_length(). 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return the number of bytes left in the FIFO 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallshort inv_get_fifo_count(void) 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return fifo_objHW.fifoCount; 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Read data from the fifo 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data Location to store the date read from the fifo 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] len Amount of data to read out of the fifo 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len) 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read_fifo(inv_get_serial_handle(), 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_mpu_slave_addr(), 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned short)len, data); 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("inv_serial_readBurst failed %d\n", result); 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_fifo(); 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA; 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Clears the FIFO status and its content. 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @note Halt the DMP writing into the FIFO for the time 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * needed to reset the FIFO. 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_reset_fifo(void) 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int len = FIFO_HW_SIZE; 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char fifoBuf[2]; 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tries = 0; 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char userCtrlReg; 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_objHW.fifoCount = 0; 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->gyro_is_suspended) 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_USER_CTRL, 1, &userCtrlReg); 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (len != 0 && tries < 6) { 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_single_write(inv_get_serial_handle(), 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_mpu_slave_addr(), MPUREG_USER_CTRL, 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((userCtrlReg & (~BIT_FIFO_EN)) | 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall BIT_FIFO_RST)); 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_FIFO_COUNTH, 2, fifoBuf); 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1]; 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tries++; 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_single_write(inv_get_serial_handle(), 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_mpu_slave_addr(), MPUREG_USER_CTRL, 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall userCtrlReg); 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @} 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 329