/* $License: Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. $ */ /****************************************************************************** * * $Id: mldl.c 5773 2011-07-14 22:28:32Z 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 #include "mpu.h" #if defined CONFIG_MPU_SENSORS_MPU6050A2 # include "mpu6050a2.h" #elif defined CONFIG_MPU_SENSORS_MPU6050B1 # include "mpu6050b1.h" #elif defined CONFIG_MPU_SENSORS_MPU3050 # include "mpu3050.h" #else #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx #endif #include "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 "mldl_cfg_init.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 */ static void *g_mlsl_handle; int_fast8_t g_int_trigger[NUM_OF_INTSOURCES]; static struct mldl_cfg *g_mldl_cfg; /******************************************************************************* * Functions for accessing the DMP memory via keys ******************************************************************************/ unsigned short (*p_get_dmp_address) (unsigned short key) = NULL; struct mpu_ram g_original_ram = { 0, 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; p_get_dmp_address = 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 (p_get_dmp_address == NULL) { MPL_LOGE("%s : p_get_dmp_address is NULL\n", __func__); return false; } memAddr = p_get_dmp_address(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 (p_get_dmp_address == NULL) { return INV_ERROR_NOT_OPENED; } offset = p_get_dmp_address(key); if (offset >= g_original_ram.length || (offset + length) > (g_original_ram.length)) { return INV_ERROR_INVALID_PARAMETER; } memcpy(buffer, &g_original_ram.ram[offset], length); return INV_SUCCESS; } unsigned short inv_dl_get_address(unsigned short key) { unsigned short offset; if (p_get_dmp_address == NULL) { return INV_ERROR_NOT_OPENED; } offset = p_get_dmp_address(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 *mlsl_handle) { inv_error_t result; memset(g_int_trigger, INT_CLEAR, sizeof(g_int_trigger)); g_mlsl_handle = mlsl_handle; result = inv_mldl_cfg_init(&g_mldl_cfg); if (result) { LOG_RESULT_LOCATION(result); inv_mldl_cfg_exit(&g_mldl_cfg); return result; } result = (inv_error_t) inv_mpu_open( g_mldl_cfg, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle); if (result) { LOG_RESULT_LOCATION(result); inv_mldl_cfg_exit(&g_mldl_cfg); } MPL_LOGD("inv_mpu_open() returns %d\n", result); 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(g_mldl_cfg, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, INV_ALL_SENSORS); result = (inv_error_t) inv_mpu_close( g_mldl_cfg, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle); /* Clear all previous settings */ inv_mldl_cfg_exit(&g_mldl_cfg); g_mlsl_handle = NULL; p_get_dmp_address = 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) { g_mldl_cfg->inv_mpu_cfg->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; g_mldl_cfg->inv_mpu_cfg->requested_sensors = sensors; result = inv_mpu_resume(g_mldl_cfg, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, 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(g_mldl_cfg, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, g_mlsl_handle, 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 g_mldl_cfg; } /** * @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 g_mldl_cfg->mpu_chip_info->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; g_mldl_cfg->mpu_gyro_cfg->dmp_enable = enableRun; g_mldl_cfg->mpu_gyro_cfg->fifo_enable = enableFIFO; g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; 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; #if defined CONFIG_MPU_SENSORS_MPU3050 /* 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; } #endif g_mldl_cfg->mpu_gyro_cfg->int_config = triggers; if (!(g_mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED)) { result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_INT_CFG, (g_mldl_cfg->mpu_gyro_cfg->int_config | g_mldl_cfg->pdata->int_config)); } else { g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; } 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; } g_mldl_cfg->mpu_gyro_cfg->lpf = lpf; g_mldl_cfg->mpu_gyro_cfg->divider = divider; g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; 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) g_mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_250DPS; else if (fullScale == 500.f) g_mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_500DPS; else if (fullScale == 1000.f) g_mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_1000DPS; else if (fullScale == 2000.f) g_mldl_cfg->mpu_gyro_cfg->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; } g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; 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; } g_mldl_cfg->mpu_gyro_cfg->ext_sync = extSync; g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; return INV_SUCCESS; } inv_error_t inv_set_ignore_system_suspend(unsigned char ignore) { INVENSENSE_FUNC_START; g_mldl_cfg->inv_mpu_cfg->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; } g_mldl_cfg->mpu_gyro_cfg->clk_src = clkSource; g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; 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) { int ii; inv_error_t result; for (ii = 0; ii < ARRAY_SIZE(g_mldl_cfg->mpu_offsets->tc); ii++) { g_mldl_cfg->mpu_offsets->tc[ii] = tc[ii]; } if (!(g_mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED)) { #ifdef CONFIG_MPU_SENSORS_MPU3050 result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_XG_OFFS_TC, tc[0]); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_YG_OFFS_TC, tc[1]); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_ZG_OFFS_TC, tc[2]); if (result) { LOG_RESULT_LOCATION(result); return result; } #else unsigned char reg; result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_XG_OFFS_TC, ((g_mldl_cfg->mpu_offsets->tc[0] << 1) & BITS_XG_OFFS_TC)); if (result) { LOG_RESULT_LOCATION(result); return result; } reg = ((g_mldl_cfg->mpu_offsets->tc[1] << 1) & BITS_YG_OFFS_TC); #ifdef CONFIG_MPU_SENSORS_MPU6050B1 if (g_mldl_cfg->pdata->level_shifter) reg |= BIT_I2C_MST_VDDIO; #endif result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_YG_OFFS_TC, reg); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, MPUREG_ZG_OFFS_TC, ((g_mldl_cfg->mpu_offsets->tc[2] << 1) & BITS_ZG_OFFS_TC)); if (result) { LOG_RESULT_LOCATION(result); return result; } #endif } else { g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; } 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]; int ii; long sf; MPL_LOGI("%s(%04x, %04x, %04x); from %04x, %04x %04x\n", __func__, offset[0], offset[1], offset[2], g_mldl_cfg->mpu_offsets->gyro[0], g_mldl_cfg->mpu_offsets->gyro[1], g_mldl_cfg->mpu_offsets->gyro[2]); sf = (2000L * 131) / g_mldl_cfg->mpu_chip_info->gyro_sens_trim; for (ii = 0; ii < ARRAY_SIZE(g_mldl_cfg->mpu_offsets->gyro); ii++) { // Record the bias in the units the register uses g_mldl_cfg->mpu_offsets->gyro[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 (!(g_mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED)) { regs[0] = MPUREG_X_OFFS_USRH; result = inv_serial_write(g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, 7, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } } else { g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; } 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 (g_mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) { if (INV_CACHE_DMP == 0) { MPL_LOGE("INV_CACHE_DMP == 0:" " tried to inv_get_mpu_memory while device suspended\n"); result = INV_ERROR_SERIAL_WRITE; } else { memcpy( buffer, &g_mldl_cfg->mpu_ram->ram[bank * MPU_MEM_BANK_SIZE + memAddr], length); result = INV_SUCCESS; } } else { result = inv_serial_read_mem( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->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 = 1; 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; } if (INV_CACHE_DMP != 0) { different = memcmp( &g_mldl_cfg->mpu_ram->ram[bank * MPU_MEM_BANK_SIZE + memAddr], buffer, length); memcpy(&g_mldl_cfg->mpu_ram->ram[bank * MPU_MEM_BANK_SIZE + memAddr], buffer, length); } if (!(g_mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED)) { result = inv_serial_write_mem( g_mlsl_handle, g_mldl_cfg->mpu_chip_info->addr, ((bank << 8) | memAddr), length, buffer); if (result) { LOG_RESULT_LOCATION(result); return result; } } else if (different) { g_mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG; if (INV_CACHE_DMP == 0) { MPL_LOGE("INV_CACHE_DMP == 0:" " tried to inv_set_mpu_memory while device suspended\n"); result = INV_ERROR_SERIAL_WRITE; } } 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 (p_get_dmp_address == NULL) { return INV_ERROR_NOT_OPENED; } memAddr = p_get_dmp_address(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 (p_get_dmp_address == NULL) { MPL_LOGE("MLDSetMemoryMPU p_get_dmp_address is NULL\n"); return INV_ERROR_INVALID_MODULE; } memAddr = p_get_dmp_address(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; g_original_ram.ram = (unsigned char *)buffer; g_original_ram.length = length; g_mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = (config >> 8); g_mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = (config & 0xff); result = inv_mpu_set_firmware(g_mldl_cfg, g_mlsl_handle, buffer, length); 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 g_mldl_cfg->mpu_chip_info->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 g_mldl_cfg->mpu_chip_info->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(g_mlsl_handle, g_mldl_cfg->mpu_chip_info->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 g_int_trigger[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; g_int_trigger[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 ----*/ g_int_trigger[intSource] = INT_TRIGGERED; #ifdef ML_USE_DMP_SIM if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { MLSimHWDataInput(); } #endif return INV_SUCCESS; } /***************************/ /**@}*//* end of defgroup */ /***************************/