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: mldl.c 5653 2011-06-16 21:06:55Z nroyer $ 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *****************************************************************************/ 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup MLDL 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Motion Library - Driver Layer. 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The Motion Library Driver Layer provides the intrface to the 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * system drivers that are used by the Motion Library. 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file mldl.c 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The Motion Library Driver Layer. 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ------------------ */ 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Include Files. - */ 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ------------------ */ 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mpu.h" 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu6050a2.h" 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#elif defined CONFIG_MPU_SENSORS_MPU6050B1 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu6050b1.h" 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#elif defined CONFIG_MPU_SENSORS_MPU3050 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu3050.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlinclude.h" 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpKey.h" 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFOHW.h" 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "pressure.h" 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-mldl" 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define _mldlDebug(x) //{x} 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------------- */ 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Variables. - */ 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------------- */ 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */ 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/*---- structure containing control variables used by MLDL ----*/ 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic struct mldl_cfg mldlCfg; 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct ext_slave_descr gAccel; 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct ext_slave_descr gCompass; 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct ext_slave_descr gPressure; 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct mpu_platform_data gPdata; 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic void *sMLSLHandle; 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint_fast8_t intTrigger[NUM_OF_INTSOURCES]; 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/******************************************************************************* 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Functions for accessing the DMP memory via keys 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ******************************************************************************/ 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned short (*sGetAddress) (unsigned short key) = NULL; 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic const unsigned char *localDmpMemory = NULL; 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned short localDmpMemorySize = 0; 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Sets the function to use to convert keys to addresses. This 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * will changed for each DMP code loaded. 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param func 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Function used to convert keys to addresses. 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_set_get_address(unsigned short (*func) (unsigned short key)) 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func); 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ) 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sGetAddress = func; 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Check if the feature is supported in the currently loaded 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * DMP code basing on the fact that the key is assigned a 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * value or not. 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param key the DMP key 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return whether the feature associated with the key is supported 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or not. 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgralluint_fast8_t inv_dmpkey_supported(unsigned short key) 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short memAddr; 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sGetAddress == NULL) { 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("%s : sGetAddress is NULL\n", __func__); 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return FALSE; 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr = sGetAddress(key); 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (memAddr >= 0xffff) { 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("inv_set_mpu_memory unsupported key\n"); 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return FALSE; 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return TRUE; 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief used to get the specified number of bytes from the original 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * MPU memory location specified by the key. 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Reads the specified number of bytes from the MPU location 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * that was used to program the MPU specified by the key. Each 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * set of code specifies a function that changes keys into 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * addresses. This function is set with setGetAddress(). 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param key The key to use when looking up the address. 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length Number of bytes to read. 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer Result for data. 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * not corresponding to a memory address will result in INV_ERROR. 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_mpu_memory_original(unsigned short key, 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short length, 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *buffer) 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short offset; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sGetAddress == NULL) { 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_NOT_OPENED; 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall offset = sGetAddress(key); 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) { 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memcpy(buffer, &localDmpMemory[offset], length); 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned short inv_dl_get_address(unsigned short key) 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short offset; 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sGetAddress == NULL) { 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_NOT_OPENED; 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall offset = sGetAddress(key); 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return offset; 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ---------------------- */ 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Static Functions. - */ 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ---------------------- */ 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Open the driver layer and resets the internal 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * gyroscope, accelerometer, and compass data 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * structures. 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param mlslHandle 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the serial handle. 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_dl_open(void *mlslHandle) 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&mldlCfg, 0, sizeof(mldlCfg)); 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(intTrigger, INT_CLEAR, sizeof(intTrigger)); 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle = mlslHandle; 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */ 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.accel = &gAccel; 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.compass = &gCompass; 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.pressure = &gPressure; 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.pdata = &gPdata; 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle, 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, sMLSLHandle, sMLSLHandle); 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Closes/Cleans up the ML Driver Layer. 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Put the device in sleep mode. 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code. 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_dl_close(void) 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = (inv_error_t) inv_mpu_suspend(&mldlCfg, 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_ALL_SENSORS); 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle, 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, sMLSLHandle, sMLSLHandle); 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Clear all previous settings */ 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&mldlCfg, 0, sizeof(mldlCfg)); 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle = NULL; 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sGetAddress = NULL; 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Sets the requested_sensors 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Accessor to set the requested_sensors field of the mldl_cfg structure. 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Typically set at initialization. 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param sensors 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Bitfield of the sensors that are going to be used. Combination of the 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * following: 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_GYRO 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_GYRO 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_GYRO 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_DMP_PROCESSOR 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_ACCEL 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_ACCEL 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_ACCEL 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_COMPASS 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_COMPASS 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_COMPASS 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_PRESSURE 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_PRESSURE 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_PRESSURE 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_GYRO 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_ACCEL 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_COMPASS 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_PRESSURE 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_init_requested_sensors(unsigned long sensors) 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.requested_sensors = sensors; 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Starts the DMP running 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Resumes the sensor if any of the sensor axis or components are requested 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param sensors 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Bitfield of the sensors to turn on. Combination of the following: 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_GYRO 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_GYRO 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_GYRO 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_DMP_PROCESSOR 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_ACCEL 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_ACCEL 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_ACCEL 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_COMPASS 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_COMPASS 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_COMPASS 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_PRESSURE 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_PRESSURE 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_PRESSURE 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_GYRO 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_ACCEL 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_COMPASS 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_PRESSURE 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_dl_start(unsigned long sensors) 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.requested_sensors = sensors; 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_resume(&mldlCfg, 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sensors); 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Stops the DMP running and puts it in low power as requested 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Suspends each sensor according to the bitfield, if all axis and components 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of the sensor is off. 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param sensors Bitfiled of the sensors to leave on. Combination of the 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * following: 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_GYRO 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_GYRO 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_GYRO 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_ACCEL 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_ACCEL 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_ACCEL 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_COMPASS 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_COMPASS 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_COMPASS 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_X_PRESSURE 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Y_PRESSURE 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_Z_PRESSURE 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_GYRO 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_ACCEL 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_COMPASS 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_THREE_AXIS_PRESSURE 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_dl_stop(unsigned long sensors) 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_suspend(&mldlCfg, 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sMLSLHandle, 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sensors); 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get a pointer to the internal data structure 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * storing the configuration for the MPU, the accelerometer 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and the compass in use. 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return a pointer to the data structure of type 'struct mldl_cfg'. 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct mldl_cfg *inv_get_dl_config(void) 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return &mldlCfg; 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Query the MPU slave address. 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return The 7-bit mpu slave address. 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char inv_get_mpu_slave_addr(void) 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return mldlCfg.addr; 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief MLDLCfgDMP configures the Digital Motion Processor internal to 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the MPU. The DMP can be enabled or disabled and the start address 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * can be set. 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param enableRun Enables the DMP processing if set to TRUE. 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param enableFIFO Enables DMP output to the FIFO if set to TRUE. 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param startAddress start address 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Zero if the command is successful, an error code otherwise. 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char enableFIFO) 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.dmp_enable = enableRun; 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.fifo_enable = enableFIFO; 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin. 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The basic interrupt signal characteristics can be set 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (i.e. active high/low, open drain/push pull, etc.) and the 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * triggers can be set. 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Currently only INTPIN_MPU is supported. 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param triggers 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bitmask of triggers to enable for interrupt. 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The available triggers are: 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - BIT_MPU_RDY_EN 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - BIT_DMP_INT_EN 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - BIT_RAW_RDY_EN 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Zero if the command is successful, an error code otherwise. 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_dl_cfg_int(unsigned char triggers) 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU3050 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Mantis has 8 bits of interrupt config bits */ 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) { 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.int_config = triggers; 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mldlCfg.gyro_is_suspended) { 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_INT_CFG, 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldlCfg.int_config | mldlCfg.pdata-> 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_config)); 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief configures the output sampling rate on the MPU. 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Three parameters control the sampling: 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1) Low pass filter bandwidth, and 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2) output sampling divider. 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The output sampling rate is determined by the divider and the low 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * pass filter setting. If the low pass filter is set to 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * divider is 8kHz; for all other settings it is 1kHz. 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The 8-bit divider will divide this frequency to get the resulting 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sample frequency. 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * For example, if the filter setting is not 256Hz and the divider is 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * set to 7, then the sample rate is as follows: 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms). 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The low pass filter selection codes control both the cutoff frequency of 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the internal low pass filter and internal analog sampling rate. The 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * latter, in turn, affects the final output sampling rate according to the 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sample rate divider settig. 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate, 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate, 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate, 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate, 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate, 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate, 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate, 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate. 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param lpf low pass filter, 0 to 7. 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param divider Output sampling rate divider, 0 to 255. 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return ML_SUCESS if successful; a non-zero error code otherwise. 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider) 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- do range checking ----*/ 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lpf >= NUM_MPU_FILTER) { 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.lpf = lpf; 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.divider = divider; 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief set the full scale range for the gyros. 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The full scale selection codes correspond to: 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 0 -> 250 dps, 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1 -> 500 dps, 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2 -> 1000 dps, 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 3 -> 2000 dps. 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Full scale range affect the MPU's measurement 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sensitivity. 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param fullScale 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the gyro full scale range in dps. 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code. 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_full_scale(float fullScale) 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fullScale == 250.f) 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.full_scale = MPU_FS_250DPS; 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (fullScale == 500.f) 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.full_scale = MPU_FS_500DPS; 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (fullScale == 1000.f) 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.full_scale = MPU_FS_1000DPS; 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (fullScale == 2000.f) 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.full_scale = MPU_FS_2000DPS; 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else { // not a valid setting 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Invalid full scale range specification for gyros : %f\n", 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fullScale); 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n"); 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief This function sets the external sync for the MPU sampling. 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It can be synchronized on the LSB of any of the gyros, any of the 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * external accels, or on the temp readings. 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param extSync External sync selection, 0 to 7. 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Zero if the command is successful; an error code otherwise. 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_external_sync(unsigned char extSync) 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- do range checking ----*/ 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (extSync >= NUM_MPU_EXT_SYNC) { 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.ext_sync = extSync; 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_ignore_system_suspend(unsigned char ignore) 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.ignore_system_suspend = ignore; 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_clock_source function sets the clock source for the MPU gyro 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * processing. 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The source can be any of the following: 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - Internal 8MHz oscillator, 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - PLL with X gyro as reference, 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - PLL with Y gyro as reference, 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - PLL with Z gyro as reference, 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - PLL with external 32.768Mhz reference, or 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - PLL with external 19.2MHz reference 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * For best accuracy and timing, it is highly recommended to use one 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of the gyros as the clock source; however this gyro must be 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * enabled to use its clock (see 'MLDLPowerMgmtMPU()'). 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param clkSource Clock source selection. 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Can be one of: 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - CLK_INTERNAL, 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - CLK_PLLGYROX, 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - CLK_PLLGYROY, 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - CLK_PLLGYROZ, 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - CLK_PLLEXT32K, or 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - CLK_PLLEXT19M. 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Zero if the command is successful; an error code otherwise. 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_clock_source(unsigned char clkSource) 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- do range checking ----*/ 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (clkSource >= NUM_CLK_SEL) { 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.clk_src = clkSource; 598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Set the Temperature Compensation offset. 605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param tc 606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a pointer to the temperature compensations offset 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for the 3 gyro axes. 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_offsetTC(const unsigned char *tc) 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) { 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.offset_tc[ii] = tc[ii]; 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mldlCfg.gyro_is_suspended) { 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU3050 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_XG_OFFS_TC, tc[0]); 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_YG_OFFS_TC, tc[1]); 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_ZG_OFFS_TC, tc[2]); 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char reg; 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_XG_OFFS_TC, 643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((mldlCfg.offset_tc[0] << 1) 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall & BITS_XG_OFFS_TC)); 645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC); 650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU6050B1 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldlCfg.pdata->level_shifter) 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg |= BIT_I2C_MST_VDDIO; 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_YG_OFFS_TC, reg); 656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, 661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_ZG_OFFS_TC, 662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((mldlCfg.offset_tc[2] << 1) 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall & BITS_ZG_OFFS_TC)); 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Set the gyro offset. 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param offset 678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a pointer to the gyro offset for the 3 gyro axes. This is scaled 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * as it would be written to the hardware registers. 680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_offset(const short *offset) 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[7]; 686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long sf; 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = (2000L * 131) / mldlCfg.gyro_sens_trim; 690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) { 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Record the bias in the units the register uses 692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.offset[ii] = offset[ii]; 693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Convert the bias to 1 dps = 1<<16 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.gyro_bias[ii] = -offset[ii] * sf; 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff; 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff); 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mldlCfg.gyro_is_suspended) { 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = MPUREG_X_OFFS_USRH; 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs); 702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief used to get the specified number of bytes in the specified MPU 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * memory bank. 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The memory bank is one of the following: 717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_0, 718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_1, 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_2, or 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_3. 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param bank Memory bank to write. 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param memAddr Starting address for write. 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length Number of bytes to write. 725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer Result for data. 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return zero if the command is successful, an error code otherwise. 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_get_mpu_memory_one_bank(unsigned char bank, 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char memAddr, 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short length, unsigned char *buffer) 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((bank >= MPU_MEM_NUM_RAM_BANKS) || 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldlCfg.gyro_is_suspended) { 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memcpy(buffer, &mldlCfg.ram[bank][memAddr], length); 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = INV_SUCCESS; 746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr, 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((bank << 8) | memAddr), length, buffer); 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief used to set the specified number of bytes in the specified MPU 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * memory bank. 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The memory bank is one of the following: 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_0, 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_1, 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_2, or 766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - MPUMEM_RAM_BANK_3. 767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param bank Memory bank to write. 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param memAddr Starting address for write. 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length Number of bytes to write. 771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer Result for data. 772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return zero if the command is successful, an error code otherwise. 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, 777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short memAddr, 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short length, 779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const unsigned char *buffer) 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int different; 783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) || 785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length); 790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memcpy(&mldlCfg.ram[bank][memAddr], buffer, length); 791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mldlCfg.gyro_is_suspended) { 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr, 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((bank << 8) | memAddr), length, buffer); 794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (different) { 799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.gyro_needs_reset = TRUE; 800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief used to get the specified number of bytes from the MPU location 808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * specified by the key. 809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Reads the specified number of bytes from the MPU location 810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * specified by the key. Each set of code specifies a function 811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * that changes keys into addresses. This function is set with 812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * setGetAddress(). 813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param key The key to use when looking up the address. 815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length Number of bytes to read. 816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer Result for data. 817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key 819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * not corresponding to a memory address will result in INV_ERROR. 820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_mpu_memory(unsigned short key, 823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short length, unsigned char *buffer) 824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char bank; 826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short memAddr; 828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sGetAddress == NULL) { 830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_NOT_OPENED; 831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr = sGetAddress(key); 834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (memAddr >= 0xffff) 835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bank = memAddr >> 8; // Get Bank 837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr &= 0xff; 838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (memAddr + length > MPU_MEM_BANK_SIZE) { 840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // We cross a bank in the middle 841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; 842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, 843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sub_length, buffer); 844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) 845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bank++; 847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length -= sub_length; 848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall buffer += sub_length; 849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr = 0; 850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, 852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length, buffer); 853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief used to set the specified number of bytes from the MPU location 865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * specified by the key. 866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Set the specified number of bytes from the MPU location 867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * specified by the key. Each set of DMP code specifies a function 868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * that changes keys into addresses. This function is set with 869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * setGetAddress(). 870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param key The key to use when looking up the address. 872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length Number of bytes to write. 873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer Result for data. 874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key 876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * not corresponding to a memory address will result in INV_ERROR. 877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_mpu_memory(unsigned short key, 880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short length, 881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const unsigned char *buffer) 882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short memAddr; 885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char bank; 886895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 887895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sGetAddress == NULL) { 888895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n"); 889895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_MODULE; 890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 891895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr = sGetAddress(key); 892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (memAddr >= 0xffff) { 894895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("inv_set_mpu_memory unsupported key\n"); 895895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_MODULE; // This key not supported 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bank = (unsigned char)(memAddr >> 8); 899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr &= 0xff; 900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 901895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (memAddr + length > MPU_MEM_BANK_SIZE) { 902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // We cross a bank in the middle 903895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; 904895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 905895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer); 906895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 908895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 911895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bank++; 912895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length -= sub_length; 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall buffer += sub_length; 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr = 0; 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer); 917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 922895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Load the DMP with the given code and configuration. 926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param buffer 927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the DMP data. 928895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param length 929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the length in bytes of the DMP data. 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param config 931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the DMP configuration. 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_load_dmp(const unsigned char *buffer, 935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short length, unsigned short config) 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short toWrite; 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short memAddr = 0; 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall localDmpMemory = buffer; 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall localDmpMemorySize = length; 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.dmp_cfg1 = (config >> 8); 946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg.dmp_cfg2 = (config & 0xff); 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while (length > 0) { 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall toWrite = length; 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (toWrite > MAX_LOAD_WRITE_SIZE) 951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall toWrite = MAX_LOAD_WRITE_SIZE; 952895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite, 955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall buffer); 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall buffer += toWrite; 962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memAddr += toWrite; 963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall length -= toWrite; 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 969895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 970895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the silicon revision ID. 971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return The silicon revision ID 972895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (0 will be read if inv_mpu_open returned an error) 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char inv_get_silicon_rev(void) 975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return mldlCfg.silicon_revision; 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 979895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the product revision ID. 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return The product revision ID 982895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (0 will be read if inv_mpu_open returned an error) 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char inv_get_product_rev(void) 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return mldlCfg.product_revision; 987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 989895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/******************************************************************************* 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ******************************************************************************* 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ******************************************************************************* 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @todo these belong with an interface to the kernel driver layer 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ******************************************************************************* 994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ******************************************************************************* 995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ******************************************************************************/ 996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 997895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_get_interrupt_status returns the interrupt status from the specified 999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * interrupt pin. 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param intPin 1001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Currently only the value INTPIN_MPU is supported. 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param status 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The available statuses are: 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - BIT_MPU_RDY_EN 1005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - BIT_DMP_INT_EN 1006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - BIT_RAW_RDY_EN 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or a non-zero error code. 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_interrupt_status(unsigned char intPin, 1011895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *status) 1012895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (intPin) { 1018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case INTPIN_MPU: 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- return the MPU interrupt status ----*/ 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(sMLSLHandle, mldlCfg.addr, 1022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_INT_STATUS, 1, status); 1023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = INV_ERROR_INVALID_PARAMETER; 1027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1028895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1029895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1030895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief query the current status of an interrupt source. 1035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param srcIndex 1036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * index of the interrupt source. 1037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Currently the only source supported is INTPIN_MPU. 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1039895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 1 if the interrupt has been triggered. 1040895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1041895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char inv_get_interrupt_trigger(unsigned char srcIndex) 1042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return intTrigger[srcIndex]; 1045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief clear the 'triggered' status for an interrupt source. 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param srcIndex 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * index of the interrupt source. 1051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Currently only INTPIN_MPU is supported. 1052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1053895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_clear_interrupt_trigger(unsigned char srcIndex) 1054895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1056895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall intTrigger[srcIndex] = 0; 1057895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1058895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1060895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_interrupt_handler function should be called when an interrupt is 1061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * received. The source parameter identifies which interrupt source 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * caused the interrupt. Note that this routine should not be called 1063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * directly from the interrupt service routine. 1064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1, 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * INTSRC_AUX2, or INT_SRC_TIMER. 1067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Zero if the command is successful; an error code otherwise. 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1070895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_interrupt_handler(unsigned char intSource) 1071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1073895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- range check ----*/ 1074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (intSource >= NUM_OF_INTSOURCES) { 1075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 1076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- save source of interrupt ----*/ 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall intTrigger[intSource] = INT_TRIGGERED; 1080895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1081895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef ML_USE_DMP_SIM 1082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { 1083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MLSimHWDataInput(); 1084895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1087895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/***************************/ 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /**@}*//* end of defgroup */ 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/***************************/ 1093