Home | History | Annotate | Download | only in mllite 
      1 /*
      2  $License:
      3    Copyright 2011 InvenSense, Inc.
      4 
      5  Licensed under the Apache License, Version 2.0 (the "License");
      6  you may not use this file except in compliance with the License.
      7  You may obtain a copy of the License at
      8 
      9  http://www.apache.org/licenses/LICENSE-2.0
     10 
     11  Unless required by applicable law or agreed to in writing, software
     12  distributed under the License is distributed on an "AS IS" BASIS,
     13  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     14  See the License for the specific language governing permissions and
     15  limitations under the License.
     16   $
     17  */
     18 /*******************************************************************************
     19  *
     20  * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
     21  *
     22  *******************************************************************************/
     23 
     24 /**
     25  *  @defgroup MLFIFO_HW
     26  *  @brief  Motion Library - FIFO HW Driver.
     27  *          Provides facilities to interact with the FIFO.
     28  *
     29  *  @{
     30  *      @file   mlFIFOHW.c
     31  *      @brief  The Motion Library Fifo Hardware Layer.
     32  *
     33  */
     34 
     35 #include <string.h>
     36 
     37 #include "mpu.h"
     38 #if defined CONFIG_MPU_SENSORS_MPU6050A2
     39 #    include "mpu6050a2.h"
     40 #elif defined CONFIG_MPU_SENSORS_MPU6050B1
     41 #    include "mpu6050b1.h"
     42 #elif defined CONFIG_MPU_SENSORS_MPU3050
     43 #  include "mpu3050.h"
     44 #else
     45 #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
     46 #endif
     47 #include "mlFIFOHW.h"
     48 #include "ml.h"
     49 #include "mldl.h"
     50 #include "mldl_cfg.h"
     51 
     52 #include "mlsl.h"
     53 
     54 #include "log.h"
     55 #undef MPL_LOG_TAG
     56 #define MPL_LOG_TAG "MPL-fifo"
     57 
     58 /*
     59     Defines
     60 */
     61 
     62 #define _fifoDebug(x)           //{x}
     63 
     64 /*
     65     Typedefs
     66 */
     67 
     68 struct fifo_hw_obj {
     69     short fifoCount;
     70     inv_error_t fifoError;
     71     unsigned char fifoOverflow;
     72     unsigned char fifoResetOnOverflow;
     73 };
     74 
     75 /*
     76     Global variables
     77 */
     78 const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
     79 
     80 /*
     81     Static variables
     82 */
     83 static struct fifo_hw_obj fifo_objHW;
     84 
     85 /*
     86     Definitions
     87 */
     88 
     89 /**
     90  *  @brief  Initializes the internal FIFO data structure.
     91  */
     92 void inv_init_fifo_hardare(void)
     93 {
     94     memset(&fifo_objHW, 0, sizeof(fifo_objHW));
     95     fifo_objHW.fifoResetOnOverflow = TRUE;
     96 }
     97 
     98 /**
     99  *  @internal
    100  *  @brief  used to get the FIFO data.
    101  *  @param  length
    102  *              Number of bytes to read from the FIFO.
    103  *  @param  buffer
    104  *              the bytes of FIFO data.
    105  *              Note that this buffer <b>must</b> be large enough
    106  *              to store and additional trailing FIFO footer when
    107  *              expected.  The callers must make sure enough space
    108  *              is allocated.
    109  *  @return number of valid bytes of data.
    110 **/
    111 uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
    112 {
    113     INVENSENSE_FUNC_START;
    114     inv_error_t result;
    115     uint_fast16_t inFifo;
    116     uint_fast16_t toRead;
    117     int_fast8_t kk;
    118 
    119     toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
    120     /*---- make sure length is correct ----*/
    121     if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
    122         fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
    123         return 0;
    124     }
    125 
    126     result = inv_get_fifo_length(&inFifo);
    127     if (INV_SUCCESS != result) {
    128         fifo_objHW.fifoError = result;
    129         return 0;
    130     }
    131     // fifo_objHW.fifoCount is the footer size left in the buffer, or
    132     //      0 if this is the first time reading the fifo since it was reset
    133     if (inFifo < length + fifo_objHW.fifoCount) {
    134         fifo_objHW.fifoError = INV_SUCCESS;
    135         return 0;
    136     }
    137     // if a trailing fifo count is expected - start storing data 2 bytes before
    138     result =
    139         inv_read_fifo(fifo_objHW.fifoCount >
    140                       0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
    141     if (INV_SUCCESS != result) {
    142         fifo_objHW.fifoError = result;
    143         return 0;
    144     }
    145     // Make sure the fifo didn't overflow before or during the read
    146     result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
    147                              MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
    148     if (INV_SUCCESS != result) {
    149         fifo_objHW.fifoError = result;
    150         return 0;
    151     }
    152 
    153     if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
    154         MPL_LOGV("Resetting Fifo : Overflow\n");
    155         inv_reset_fifo();
    156         fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
    157         return 0;
    158     }
    159 
    160     /* Check the Footer value to give us a chance at making sure data
    161      * didn't get corrupted */
    162     for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
    163         if (buffer[kk] != gFifoFooter[kk]) {
    164             MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
    165                      buffer[0], buffer[1]);
    166             _fifoDebug(char out[200];
    167                        MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
    168                        sprintf(out, "0x");
    169                        for (kk = 0; kk < (int)toRead; kk++) {
    170                        sprintf(out, "%s%02X", out, buffer[kk]);}
    171                        MPL_LOGW("%s\n", out);)
    172                 inv_reset_fifo();
    173             fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
    174             return 0;
    175         }
    176     }
    177 
    178     if (fifo_objHW.fifoCount == 0) {
    179         fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
    180     }
    181 
    182     return length - FIFO_FOOTER_SIZE;
    183 }
    184 
    185 /**
    186  *  @brief  Used to query the status of the FIFO.
    187  *  @return INV_SUCCESS if the fifo is OK. An error code otherwise.
    188 **/
    189 inv_error_t inv_get_fifo_status(void)
    190 {
    191     inv_error_t fifoError = fifo_objHW.fifoError;
    192     fifo_objHW.fifoError = 0;
    193     return fifoError;
    194 }
    195 
    196 /**
    197  * @internal
    198  * @brief   Get the length from the fifo
    199  *
    200  * @param[out] len amount of data currently stored in the fifo.
    201  *
    202  * @return INV_SUCCESS or non-zero error code.
    203 **/
    204 inv_error_t inv_get_fifo_length(uint_fast16_t * len)
    205 {
    206     INVENSENSE_FUNC_START;
    207     unsigned char fifoBuf[2];
    208     inv_error_t result;
    209 
    210     if (NULL == len)
    211         return INV_ERROR_INVALID_PARAMETER;
    212 
    213     /*---- read the 2 'count' registers and
    214       burst read the data from the FIFO ----*/
    215     result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
    216                              MPUREG_FIFO_COUNTH, 2, fifoBuf);
    217     if (INV_SUCCESS != result) {
    218         MPL_LOGE("ReadBurst failed %d\n", result);
    219         inv_reset_fifo();
    220         fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
    221         *len = 0;
    222         return result;
    223     }
    224 
    225     *len = (uint_fast16_t) (fifoBuf[0] << 8);
    226     *len += (uint_fast16_t) (fifoBuf[1]);
    227     return result;
    228 }
    229 
    230 /**
    231  *  @brief  inv_get_fifo_count is used to get the number of bytes left in the FIFO.
    232  *          This function returns the stored value and does not access the hardware.
    233  *          See inv_get_fifo_length().
    234  *  @return the number of bytes left in the FIFO
    235 **/
    236 short inv_get_fifo_count(void)
    237 {
    238     return fifo_objHW.fifoCount;
    239 }
    240 
    241 /**
    242  *  @internal
    243  *  @brief  Read data from the fifo
    244  *
    245  *  @param[out] data Location to store the date read from the fifo
    246  *  @param[in] len   Amount of data to read out of the fifo
    247  *
    248  *  @return INV_SUCCESS or non-zero error code
    249 **/
    250 inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
    251 {
    252     INVENSENSE_FUNC_START;
    253     inv_error_t result;
    254     result = inv_serial_read_fifo(inv_get_serial_handle(),
    255                                   inv_get_mpu_slave_addr(),
    256                                   (unsigned short)len, data);
    257     if (INV_SUCCESS != result) {
    258         MPL_LOGE("inv_serial_readBurst failed %d\n", result);
    259         inv_reset_fifo();
    260         fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
    261         return result;
    262     }
    263     return result;
    264 }
    265 
    266 /**
    267  *  @brief  Clears the FIFO status and its content.
    268  *  @note   Halt the DMP writing into the FIFO for the time
    269  *          needed to reset the FIFO.
    270  *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
    271  */
    272 inv_error_t inv_reset_fifo(void)
    273 {
    274     INVENSENSE_FUNC_START;
    275     int len = FIFO_HW_SIZE;
    276     unsigned char fifoBuf[2];
    277     unsigned char tries = 0;
    278     unsigned char userCtrlReg;
    279     inv_error_t result;
    280     struct mldl_cfg *mldl_cfg = inv_get_dl_config();
    281 
    282     fifo_objHW.fifoCount = 0;
    283     if (mldl_cfg->gyro_is_suspended)
    284         return INV_SUCCESS;
    285 
    286     result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
    287                              MPUREG_USER_CTRL, 1, &userCtrlReg);
    288     if (result) {
    289         LOG_RESULT_LOCATION(result);
    290         return result;
    291     }
    292 
    293     while (len != 0 && tries < 6) {
    294         result =
    295             inv_serial_single_write(inv_get_serial_handle(),
    296                                     inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
    297                                     ((userCtrlReg & (~BIT_FIFO_EN)) |
    298                                      BIT_FIFO_RST));
    299         if (result) {
    300             LOG_RESULT_LOCATION(result);
    301             return result;
    302         }
    303         result =
    304             inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
    305                             MPUREG_FIFO_COUNTH, 2, fifoBuf);
    306         if (result) {
    307             LOG_RESULT_LOCATION(result);
    308             return result;
    309         }
    310         len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
    311         tries++;
    312     }
    313 
    314     result =
    315         inv_serial_single_write(inv_get_serial_handle(),
    316                                 inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
    317                                 userCtrlReg);
    318     if (result) {
    319         LOG_RESULT_LOCATION(result);
    320         return result;
    321     }
    322 
    323     return INV_SUCCESS;
    324 }
    325 
    326 /**
    327  * @}
    328 **/
    329