diff options
Diffstat (limited to 'invensense/mlsdk/mllite/mldl.c')
| -rw-r--r-- | invensense/mlsdk/mllite/mldl.c | 1051 |
1 files changed, 0 insertions, 1051 deletions
diff --git a/invensense/mlsdk/mllite/mldl.c b/invensense/mlsdk/mllite/mldl.c deleted file mode 100644 index ee7258f..0000000 --- a/invensense/mlsdk/mllite/mldl.c +++ /dev/null @@ -1,1051 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: mldl.c 5653 2011-06-16 21:06:55Z nroyer $ - * - *****************************************************************************/ - -/** - * @defgroup MLDL - * @brief Motion Library - Driver Layer. - * The Motion Library Driver Layer provides the intrface to the - * system drivers that are used by the Motion Library. - * - * @{ - * @file mldl.c - * @brief The Motion Library Driver Layer. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> - -#include "mpu.h" -#include "mpu3050.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "compass.h" -#include "mlsl.h" -#include "mlos.h" -#include "mlinclude.h" -#include "ml.h" -#include "dmpKey.h" -#include "mlFIFOHW.h" -#include "compass.h" -#include "pressure.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mldl" - -#define _mldlDebug(x) //{x} - -/* --------------------- */ -/* - Variables. - */ -/* --------------------- */ - -#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */ - -/*---- structure containing control variables used by MLDL ----*/ -static struct mldl_cfg mldlCfg; -struct ext_slave_descr gAccel; -struct ext_slave_descr gCompass; -struct ext_slave_descr gPressure; -struct mpu_platform_data gPdata; -static void *sMLSLHandle; -int_fast8_t intTrigger[NUM_OF_INTSOURCES]; - -/******************************************************************************* - * Functions for accessing the DMP memory via keys - ******************************************************************************/ - -unsigned short (*sGetAddress) (unsigned short key) = NULL; -static const unsigned char *localDmpMemory = NULL; -static unsigned short localDmpMemorySize = 0; - -/** - * @internal - * @brief Sets the function to use to convert keys to addresses. This - * will changed for each DMP code loaded. - * @param func - * Function used to convert keys to addresses. - * @endif - */ -void inv_set_get_address(unsigned short (*func) (unsigned short key)) -{ - INVENSENSE_FUNC_START; - _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func); - ) - sGetAddress = func; -} - -/** - * @internal - * @brief Check if the feature is supported in the currently loaded - * DMP code basing on the fact that the key is assigned a - * value or not. - * @param key the DMP key - * @return whether the feature associated with the key is supported - * or not. - */ -uint_fast8_t inv_dmpkey_supported(unsigned short key) -{ - unsigned short memAddr; - - if (sGetAddress == NULL) { - MPL_LOGE("%s : sGetAddress is NULL\n", __func__); - return FALSE; - } - - memAddr = sGetAddress(key); - if (memAddr >= 0xffff) { - MPL_LOGV("inv_set_mpu_memory unsupported key\n"); - return FALSE; - } - - return TRUE; -} - -/** - * @internal - * @brief used to get the specified number of bytes from the original - * MPU memory location specified by the key. - * Reads the specified number of bytes from the MPU location - * that was used to program the MPU specified by the key. Each - * set of code specifies a function that changes keys into - * addresses. This function is set with setGetAddress(). - * - * @param key The key to use when looking up the address. - * @param length Number of bytes to read. - * @param buffer Result for data. - * - * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key - * not corresponding to a memory address will result in INV_ERROR. - * @endif - */ -inv_error_t inv_get_mpu_memory_original(unsigned short key, - unsigned short length, - unsigned char *buffer) -{ - unsigned short offset; - - if (sGetAddress == NULL) { - return INV_ERROR_NOT_OPENED; - } - - offset = sGetAddress(key); - if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) { - return INV_ERROR_INVALID_PARAMETER; - } - - memcpy(buffer, &localDmpMemory[offset], length); - - return INV_SUCCESS; -} - -unsigned short inv_dl_get_address(unsigned short key) -{ - unsigned short offset; - if (sGetAddress == NULL) { - return INV_ERROR_NOT_OPENED; - } - - offset = sGetAddress(key); - return offset; -} - -/* ---------------------- */ -/* - Static Functions. - */ -/* ---------------------- */ - -/** - * @brief Open the driver layer and resets the internal - * gyroscope, accelerometer, and compass data - * structures. - * @param mlslHandle - * the serial handle. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_dl_open(void *mlslHandle) -{ - inv_error_t result; - memset(&mldlCfg, 0, sizeof(mldlCfg)); - memset(intTrigger, INT_CLEAR, sizeof(intTrigger)); - - sMLSLHandle = mlslHandle; - - mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */ - mldlCfg.accel = &gAccel; - mldlCfg.compass = &gCompass; - mldlCfg.pressure = &gPressure; - mldlCfg.pdata = &gPdata; - - result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle, - sMLSLHandle, sMLSLHandle, sMLSLHandle); - return result; -} - -/** - * @brief Closes/Cleans up the ML Driver Layer. - * Put the device in sleep mode. - * @return INV_SUCCESS or non-zero error code. - */ -inv_error_t inv_dl_close(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - result = (inv_error_t) inv_mpu_suspend(&mldlCfg, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - INV_ALL_SENSORS); - - result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle, - sMLSLHandle, sMLSLHandle, sMLSLHandle); - /* Clear all previous settings */ - memset(&mldlCfg, 0, sizeof(mldlCfg)); - sMLSLHandle = NULL; - sGetAddress = NULL; - return result; -} - -/** - * @brief Sets the requested_sensors - * - * Accessor to set the requested_sensors field of the mldl_cfg structure. - * Typically set at initialization. - * - * @param sensors - * Bitfield of the sensors that are going to be used. Combination of the - * following: - * - INV_X_GYRO - * - INV_Y_GYRO - * - INV_Z_GYRO - * - INV_DMP_PROCESSOR - * - INV_X_ACCEL - * - INV_Y_ACCEL - * - INV_Z_ACCEL - * - INV_X_COMPASS - * - INV_Y_COMPASS - * - INV_Z_COMPASS - * - INV_X_PRESSURE - * - INV_Y_PRESSURE - * - INV_Z_PRESSURE - * - INV_THREE_AXIS_GYRO - * - INV_THREE_AXIS_ACCEL - * - INV_THREE_AXIS_COMPASS - * - INV_THREE_AXIS_PRESSURE - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_init_requested_sensors(unsigned long sensors) -{ - mldlCfg.requested_sensors = sensors; - - return INV_SUCCESS; -} - -/** - * @brief Starts the DMP running - * - * Resumes the sensor if any of the sensor axis or components are requested - * - * @param sensors - * Bitfield of the sensors to turn on. Combination of the following: - * - INV_X_GYRO - * - INV_Y_GYRO - * - INV_Z_GYRO - * - INV_DMP_PROCESSOR - * - INV_X_ACCEL - * - INV_Y_ACCEL - * - INV_Z_ACCEL - * - INV_X_COMPASS - * - INV_Y_COMPASS - * - INV_Z_COMPASS - * - INV_X_PRESSURE - * - INV_Y_PRESSURE - * - INV_Z_PRESSURE - * - INV_THREE_AXIS_GYRO - * - INV_THREE_AXIS_ACCEL - * - INV_THREE_AXIS_COMPASS - * - INV_THREE_AXIS_PRESSURE - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_dl_start(unsigned long sensors) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - mldlCfg.requested_sensors = sensors; - result = inv_mpu_resume(&mldlCfg, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sensors); - return result; -} - -/** - * @brief Stops the DMP running and puts it in low power as requested - * - * Suspends each sensor according to the bitfield, if all axis and components - * of the sensor is off. - * - * @param sensors Bitfiled of the sensors to leave on. Combination of the - * following: - * - INV_X_GYRO - * - INV_Y_GYRO - * - INV_Z_GYRO - * - INV_X_ACCEL - * - INV_Y_ACCEL - * - INV_Z_ACCEL - * - INV_X_COMPASS - * - INV_Y_COMPASS - * - INV_Z_COMPASS - * - INV_X_PRESSURE - * - INV_Y_PRESSURE - * - INV_Z_PRESSURE - * - INV_THREE_AXIS_GYRO - * - INV_THREE_AXIS_ACCEL - * - INV_THREE_AXIS_COMPASS - * - INV_THREE_AXIS_PRESSURE - * - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_dl_stop(unsigned long sensors) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - result = inv_mpu_suspend(&mldlCfg, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sensors); - return result; -} - -/** - * @brief Get a pointer to the internal data structure - * storing the configuration for the MPU, the accelerometer - * and the compass in use. - * @return a pointer to the data structure of type 'struct mldl_cfg'. - */ -struct mldl_cfg *inv_get_dl_config(void) -{ - return &mldlCfg; -} - -/** - * @brief Query the MPU slave address. - * @return The 7-bit mpu slave address. - */ -unsigned char inv_get_mpu_slave_addr(void) -{ - INVENSENSE_FUNC_START; - return mldlCfg.addr; -} - -/** - * @internal - * @brief MLDLCfgDMP configures the Digital Motion Processor internal to - * the MPU. The DMP can be enabled or disabled and the start address - * can be set. - * - * @param enableRun Enables the DMP processing if set to TRUE. - * @param enableFIFO Enables DMP output to the FIFO if set to TRUE. - * @param startAddress start address - * - * @return Zero if the command is successful, an error code otherwise. -*/ -inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, - unsigned char enableFIFO) -{ - INVENSENSE_FUNC_START; - - mldlCfg.dmp_enable = enableRun; - mldlCfg.fifo_enable = enableFIFO; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin. - * The basic interrupt signal characteristics can be set - * (i.e. active high/low, open drain/push pull, etc.) and the - * triggers can be set. - * Currently only INTPIN_MPU is supported. - * - * @param triggers - * bitmask of triggers to enable for interrupt. - * The available triggers are: - * - BIT_MPU_RDY_EN - * - BIT_DMP_INT_EN - * - BIT_RAW_RDY_EN - * - * @return Zero if the command is successful, an error code otherwise. -*/ -inv_error_t inv_get_dl_cfg_int(unsigned char triggers) -{ - inv_error_t result = INV_SUCCESS; - - /* Mantis has 8 bits of interrupt config bits */ - if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) { - return INV_ERROR_INVALID_PARAMETER; - } - - mldlCfg.int_config = triggers; - if (!mldlCfg.gyro_is_suspended) { - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_INT_CFG, - (mldlCfg.int_config | mldlCfg.pdata-> - int_config)); - } else { - mldlCfg.gyro_needs_reset = TRUE; - } - - return result; -} - -/** - * @brief configures the output sampling rate on the MPU. - * Three parameters control the sampling: - * - * 1) Low pass filter bandwidth, and - * 2) output sampling divider. - * - * The output sampling rate is determined by the divider and the low - * pass filter setting. If the low pass filter is set to - * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the - * divider is 8kHz; for all other settings it is 1kHz. - * The 8-bit divider will divide this frequency to get the resulting - * sample frequency. - * For example, if the filter setting is not 256Hz and the divider is - * set to 7, then the sample rate is as follows: - * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms). - * - * The low pass filter selection codes control both the cutoff frequency of - * the internal low pass filter and internal analog sampling rate. The - * latter, in turn, affects the final output sampling rate according to the - * sample rate divider settig. - * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate, - * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate, - * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate, - * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate, - * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate, - * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate, - * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate, - * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate. - * - * @param lpf low pass filter, 0 to 7. - * @param divider Output sampling rate divider, 0 to 255. - * - * @return ML_SUCESS if successful; a non-zero error code otherwise. -**/ -inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider) -{ - /*---- do range checking ----*/ - if (lpf >= NUM_MPU_FILTER) { - return INV_ERROR_INVALID_PARAMETER; - } - - mldlCfg.lpf = lpf; - mldlCfg.divider = divider; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief set the full scale range for the gyros. - * The full scale selection codes correspond to: - * 0 -> 250 dps, - * 1 -> 500 dps, - * 2 -> 1000 dps, - * 3 -> 2000 dps. - * Full scale range affect the MPU's measurement - * sensitivity. - * - * @param fullScale - * the gyro full scale range in dps. - * - * @return INV_SUCCESS or non-zero error code. -**/ -inv_error_t inv_set_full_scale(float fullScale) -{ - if (fullScale == 250.f) - mldlCfg.full_scale = MPU_FS_250DPS; - else if (fullScale == 500.f) - mldlCfg.full_scale = MPU_FS_500DPS; - else if (fullScale == 1000.f) - mldlCfg.full_scale = MPU_FS_1000DPS; - else if (fullScale == 2000.f) - mldlCfg.full_scale = MPU_FS_2000DPS; - else { // not a valid setting - MPL_LOGE("Invalid full scale range specification for gyros : %f\n", - fullScale); - MPL_LOGE - ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n"); - return INV_ERROR_INVALID_PARAMETER; - } - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief This function sets the external sync for the MPU sampling. - * It can be synchronized on the LSB of any of the gyros, any of the - * external accels, or on the temp readings. - * - * @param extSync External sync selection, 0 to 7. - * @return Zero if the command is successful; an error code otherwise. -**/ -inv_error_t inv_set_external_sync(unsigned char extSync) -{ - INVENSENSE_FUNC_START; - - /*---- do range checking ----*/ - if (extSync >= NUM_MPU_EXT_SYNC) { - return INV_ERROR_INVALID_PARAMETER; - } - mldlCfg.ext_sync = extSync; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -inv_error_t inv_set_ignore_system_suspend(unsigned char ignore) -{ - INVENSENSE_FUNC_START; - - mldlCfg.ignore_system_suspend = ignore; - - return INV_SUCCESS; -} - -/** - * @brief inv_clock_source function sets the clock source for the MPU gyro - * processing. - * The source can be any of the following: - * - Internal 8MHz oscillator, - * - PLL with X gyro as reference, - * - PLL with Y gyro as reference, - * - PLL with Z gyro as reference, - * - PLL with external 32.768Mhz reference, or - * - PLL with external 19.2MHz reference - * - * For best accuracy and timing, it is highly recommended to use one - * of the gyros as the clock source; however this gyro must be - * enabled to use its clock (see 'MLDLPowerMgmtMPU()'). - * - * @param clkSource Clock source selection. - * Can be one of: - * - CLK_INTERNAL, - * - CLK_PLLGYROX, - * - CLK_PLLGYROY, - * - CLK_PLLGYROZ, - * - CLK_PLLEXT32K, or - * - CLK_PLLEXT19M. - * - * @return Zero if the command is successful; an error code otherwise. -**/ -inv_error_t inv_clock_source(unsigned char clkSource) -{ - INVENSENSE_FUNC_START; - - /*---- do range checking ----*/ - if (clkSource >= NUM_CLK_SEL) { - return INV_ERROR_INVALID_PARAMETER; - } - - mldlCfg.clk_src = clkSource; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief Set the Temperature Compensation offset. - * @param tc - * a pointer to the temperature compensations offset - * for the 3 gyro axes. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_set_offsetTC(const unsigned char *tc) -{ - unsigned int ii; - inv_error_t result; - - for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) { - mldlCfg.offset_tc[ii] = tc[ii]; - } - - if (!mldlCfg.gyro_is_suspended) { - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_XG_OFFS_TC, tc[0]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_YG_OFFS_TC, tc[1]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_ZG_OFFS_TC, tc[2]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else { - mldlCfg.gyro_needs_reset = TRUE; - } - return INV_SUCCESS; -} - -/** - * @brief Set the gyro offset. - * @param offset - * a pointer to the gyro offset for the 3 gyro axes. This is scaled - * as it would be written to the hardware registers. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_set_offset(const short *offset) -{ - inv_error_t result; - unsigned char regs[7]; - unsigned int ii; - long sf; - - sf = (2000L * 131) / mldlCfg.gyro_sens_trim; - for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) { - // Record the bias in the units the register uses - mldlCfg.offset[ii] = offset[ii]; - // Convert the bias to 1 dps = 1<<16 - inv_obj.gyro_bias[ii] = -offset[ii] * sf; - regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff; - regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff); - } - - if (!mldlCfg.gyro_is_suspended) { - regs[0] = MPUREG_X_OFFS_USRH; - result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else { - mldlCfg.gyro_needs_reset = TRUE; - } - return INV_SUCCESS; -} - -/** - * @internal - * @brief used to get the specified number of bytes in the specified MPU - * memory bank. - * The memory bank is one of the following: - * - MPUMEM_RAM_BANK_0, - * - MPUMEM_RAM_BANK_1, - * - MPUMEM_RAM_BANK_2, or - * - MPUMEM_RAM_BANK_3. - * - * @param bank Memory bank to write. - * @param memAddr Starting address for write. - * @param length Number of bytes to write. - * @param buffer Result for data. - * - * @return zero if the command is successful, an error code otherwise. - * @endif - */ -inv_error_t -inv_get_mpu_memory_one_bank(unsigned char bank, - unsigned char memAddr, - unsigned short length, unsigned char *buffer) -{ - inv_error_t result; - - if ((bank >= MPU_MEM_NUM_RAM_BANKS) || - //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255 - ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (mldlCfg.gyro_is_suspended) { - memcpy(buffer, &mldlCfg.ram[bank][memAddr], length); - result = INV_SUCCESS; - } else { - result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr, - ((bank << 8) | memAddr), length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - return result; -} - -/** - * @internal - * @brief used to set the specified number of bytes in the specified MPU - * memory bank. - * The memory bank is one of the following: - * - MPUMEM_RAM_BANK_0, - * - MPUMEM_RAM_BANK_1, - * - MPUMEM_RAM_BANK_2, or - * - MPUMEM_RAM_BANK_3. - * - * @param bank Memory bank to write. - * @param memAddr Starting address for write. - * @param length Number of bytes to write. - * @param buffer Result for data. - * - * @return zero if the command is successful, an error code otherwise. - * @endif - */ -inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, - unsigned short memAddr, - unsigned short length, - const unsigned char *buffer) -{ - inv_error_t result = INV_SUCCESS; - int different; - - if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) || - ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { - return INV_ERROR_INVALID_PARAMETER; - } - - different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length); - memcpy(&mldlCfg.ram[bank][memAddr], buffer, length); - if (!mldlCfg.gyro_is_suspended) { - result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr, - ((bank << 8) | memAddr), length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else if (different) { - mldlCfg.gyro_needs_reset = TRUE; - } - - return result; -} - -/** - * @internal - * @brief used to get the specified number of bytes from the MPU location - * specified by the key. - * Reads the specified number of bytes from the MPU location - * specified by the key. Each set of code specifies a function - * that changes keys into addresses. This function is set with - * setGetAddress(). - * - * @param key The key to use when looking up the address. - * @param length Number of bytes to read. - * @param buffer Result for data. - * - * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key - * not corresponding to a memory address will result in INV_ERROR. - * @endif - */ -inv_error_t inv_get_mpu_memory(unsigned short key, - unsigned short length, unsigned char *buffer) -{ - unsigned char bank; - inv_error_t result; - unsigned short memAddr; - - if (sGetAddress == NULL) { - return INV_ERROR_NOT_OPENED; - } - - memAddr = sGetAddress(key); - if (memAddr >= 0xffff) - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - bank = memAddr >> 8; // Get Bank - memAddr &= 0xff; - - while (memAddr + length > MPU_MEM_BANK_SIZE) { - // We cross a bank in the middle - unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; - result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, - sub_length, buffer); - if (INV_SUCCESS != result) - return result; - bank++; - length -= sub_length; - buffer += sub_length; - memAddr = 0; - } - result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, - length, buffer); - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/** - * @internal - * @brief used to set the specified number of bytes from the MPU location - * specified by the key. - * Set the specified number of bytes from the MPU location - * specified by the key. Each set of DMP code specifies a function - * that changes keys into addresses. This function is set with - * setGetAddress(). - * - * @param key The key to use when looking up the address. - * @param length Number of bytes to write. - * @param buffer Result for data. - * - * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key - * not corresponding to a memory address will result in INV_ERROR. - * @endif - */ -inv_error_t inv_set_mpu_memory(unsigned short key, - unsigned short length, - const unsigned char *buffer) -{ - inv_error_t result = INV_SUCCESS; - unsigned short memAddr; - unsigned char bank; - - if (sGetAddress == NULL) { - MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n"); - return INV_ERROR_INVALID_MODULE; - } - memAddr = sGetAddress(key); - - if (memAddr >= 0xffff) { - MPL_LOGE("inv_set_mpu_memory unsupported key\n"); - return INV_ERROR_INVALID_MODULE; // This key not supported - } - - bank = (unsigned char)(memAddr >> 8); - memAddr &= 0xff; - - while (memAddr + length > MPU_MEM_BANK_SIZE) { - // We cross a bank in the middle - unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; - - result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - bank++; - length -= sub_length; - buffer += sub_length; - memAddr = 0; - } - result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief Load the DMP with the given code and configuration. - * @param buffer - * the DMP data. - * @param length - * the length in bytes of the DMP data. - * @param config - * the DMP configuration. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_dmp(const unsigned char *buffer, - unsigned short length, unsigned short config) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - unsigned short toWrite; - unsigned short memAddr = 0; - localDmpMemory = buffer; - localDmpMemorySize = length; - - mldlCfg.dmp_cfg1 = (config >> 8); - mldlCfg.dmp_cfg2 = (config & 0xff); - - while (length > 0) { - toWrite = length; - if (toWrite > MAX_LOAD_WRITE_SIZE) - toWrite = MAX_LOAD_WRITE_SIZE; - - result = - inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite, - buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - buffer += toWrite; - memAddr += toWrite; - length -= toWrite; - } - - return result; -} - -/** - * @brief Get the silicon revision ID. - * @return The silicon revision ID - * (0 will be read if inv_mpu_open returned an error) - */ -unsigned char inv_get_silicon_rev(void) -{ - return mldlCfg.silicon_revision; -} - -/** - * @brief Get the product revision ID. - * @return The product revision ID - * (0 will be read if inv_mpu_open returned an error) - */ -unsigned char inv_get_product_rev(void) -{ - return mldlCfg.product_revision; -} - -/******************************************************************************* - ******************************************************************************* - ******************************************************************************* - * @todo these belong with an interface to the kernel driver layer - ******************************************************************************* - ******************************************************************************* - ******************************************************************************/ - -/** - * @brief inv_get_interrupt_status returns the interrupt status from the specified - * interrupt pin. - * @param intPin - * Currently only the value INTPIN_MPU is supported. - * @param status - * The available statuses are: - * - BIT_MPU_RDY_EN - * - BIT_DMP_INT_EN - * - BIT_RAW_RDY_EN - * - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_interrupt_status(unsigned char intPin, - unsigned char *status) -{ - INVENSENSE_FUNC_START; - - inv_error_t result; - - switch (intPin) { - - case INTPIN_MPU: - /*---- return the MPU interrupt status ----*/ - result = inv_serial_read(sMLSLHandle, mldlCfg.addr, - MPUREG_INT_STATUS, 1, status); - break; - - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} - -/** - * @brief query the current status of an interrupt source. - * @param srcIndex - * index of the interrupt source. - * Currently the only source supported is INTPIN_MPU. - * - * @return 1 if the interrupt has been triggered. - */ -unsigned char inv_get_interrupt_trigger(unsigned char srcIndex) -{ - INVENSENSE_FUNC_START; - return intTrigger[srcIndex]; -} - -/** - * @brief clear the 'triggered' status for an interrupt source. - * @param srcIndex - * index of the interrupt source. - * Currently only INTPIN_MPU is supported. - */ -void inv_clear_interrupt_trigger(unsigned char srcIndex) -{ - INVENSENSE_FUNC_START; - intTrigger[srcIndex] = 0; -} - -/** - * @brief inv_interrupt_handler function should be called when an interrupt is - * received. The source parameter identifies which interrupt source - * caused the interrupt. Note that this routine should not be called - * directly from the interrupt service routine. - * - * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1, - * INTSRC_AUX2, or INT_SRC_TIMER. - * - * @return Zero if the command is successful; an error code otherwise. - */ -inv_error_t inv_interrupt_handler(unsigned char intSource) -{ - INVENSENSE_FUNC_START; - /*---- range check ----*/ - if (intSource >= NUM_OF_INTSOURCES) { - return INV_ERROR; - } - - /*---- save source of interrupt ----*/ - intTrigger[intSource] = INT_TRIGGERED; - -#ifdef ML_USE_DMP_SIM - if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { - MLSimHWDataInput(); - } -#endif - - return INV_SUCCESS; -} - -/***************************/ - /**@}*//* end of defgroup */ -/***************************/ |