/* $License: Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. $ */ /******************************************************************************* * * $Id: mlFIFO.c 5776 2011-07-15 00:16:52Z jcalizo $ * *******************************************************************************/ /** * @defgroup MLFIFO * @brief Motion Library - FIFO Driver. * The FIFO API interface. * * @{ * @file mlFIFO.c * @brief FIFO Interface. */ #include #include #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 "mlFIFO.h" #include "mlFIFOHW.h" #include "dmpKey.h" #include "mlMathFunc.h" #include "ml.h" #include "mldl.h" #include "mldl_cfg.h" #include "mlstates.h" #include "mlsupervisor.h" #include "mlos.h" #include "mlmath.h" #include "accel.h" #include "compass.h" #include "log.h" #undef MPL_LOG_TAG #define MPL_LOG_TAG "MPL-fifo" #undef MPL_LOG_NDEBUG #define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ #if (MPL_LOG_NDEBUG == 0) #undef MPL_LOGV #define MPL_LOGV(...) do { } while (0) #endif #define REF_QUATERNION (0) #define REF_GYROS (REF_QUATERNION + 4) #define REF_CONTROL (REF_GYROS + 3) #define REF_RAW (REF_CONTROL + 4) #define REF_RAW_EXTERNAL (REF_RAW + 8) #define REF_ACCEL (REF_RAW_EXTERNAL + 6) #define REF_QUANT_ACCEL (REF_ACCEL + 3) #define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) #define REF_EIS (REF_QUATERNION_6AXIS + 4) #define REF_DMP_PACKET (REF_EIS + 3) #define REF_GARBAGE (REF_DMP_PACKET + 1) #define REF_LAST (REF_GARBAGE + 1) long fifo_scale[REF_LAST] = { (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2) 1501974482L, 1501974482L, 1501974482L, // Gyro (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control (1L << 14), // Temperature (1L << 14), (1L << 14), (1L << 14), // Raw Gyro (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding (1L << 14), (1L << 14), (1L << 14), // Raw External (1L << 14), (1L << 14), (1L << 14), // Raw External (1L << 16), (1L << 16), (1L << 16), // Accel (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis (1L << 30), (1L << 30), (1L << 30), // EIS (1L << 30), // Packet (1L << 30), // Garbage }; // The scale factors for tap need to match the number in fifo_scale above. // fifo_base_offset below may also need to be changed if this is not 8 #if INV_MAX_NUM_ACCEL_SAMPLES != 8 #error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8 #endif #define CONFIG_QUAT (0) #define CONFIG_GYROS (CONFIG_QUAT + 1) #define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1) #define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1) #define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1) #define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1) #define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1) #define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1) #define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1) #define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1) #define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1) #define NUMFIFOELEMENTS (CONFIG_FOOTER + 1) const int fifo_base_offset[NUMFIFOELEMENTS] = { REF_QUATERNION * 4, REF_GYROS * 4, REF_CONTROL * 4, REF_RAW * 4, REF_RAW * 4 + 4, REF_RAW_EXTERNAL * 4, REF_ACCEL * 4, REF_QUANT_ACCEL * 4, REF_EIS * 4, REF_DMP_PACKET * 4, REF_GARBAGE * 4 }; struct fifo_obj { void (*fifo_process_cb) (void); long decoded[REF_LAST]; long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES]; int offsets[REF_LAST * 4]; int cache; uint_fast8_t gyro_source; unsigned short fifo_rate; unsigned short sample_step_size_ms; uint_fast16_t fifo_packet_size; uint_fast16_t data_config[NUMFIFOELEMENTS]; unsigned char reference_count[REF_LAST]; long acc_bias_filt[3]; float acc_filter_coef; long gravity_cache[3]; }; static struct fifo_obj fifo_obj; #define FIFO_CACHE_TEMPERATURE 1 #define FIFO_CACHE_GYRO 2 #define FIFO_CACHE_GRAVITY_BODY 4 #define FIFO_CACHE_ACC_BIAS 8 struct fifo_rate_obj_t { // These describe callbacks happening everytime a FIFO block is processed int_fast8_t num_cb; HANDLE mutex; inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES]; int priority[MAX_HIGH_RATE_PROCESSES]; }; struct fifo_rate_obj_t fifo_rate_obj; /** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old * accuracy if needed. */ static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements, uint_fast16_t accuracy, uint_fast8_t configOffset) { if (elements) { if (!accuracy) accuracy = fifo_obj.data_config[configOffset]; else if (accuracy & INV_16_BIT) if ((fifo_obj.data_config[configOffset] & INV_32_BIT)) accuracy = INV_32_BIT; // 32-bits takes priority else accuracy = INV_16_BIT; else accuracy = INV_32_BIT; } else { accuracy = 0; } return accuracy; } /** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, * the reference counts are subtracted, otherwise they are incremented for each * bit set in element. The value returned are the elements that should be sent * out as data through the FIFO. */ static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements, uint_fast16_t increment, uint_fast8_t refOffset, uint_fast8_t len) { uint_fast8_t kk; if (increment == 0) { for (kk = 0; kk < len; ++kk) { if ((elements & 1) && (fifo_obj.reference_count[kk + refOffset] > 0)) { fifo_obj.reference_count[kk + refOffset]--; } elements >>= 1; } } else { for (kk = 0; kk < len; ++kk) { if (elements & 1) fifo_obj.reference_count[kk + refOffset]++; elements >>= 1; } } elements = 0; for (kk = 0; kk < len; ++kk) { if (fifo_obj.reference_count[kk + refOffset] > 0) elements |= (1 << kk); } return elements; } /** * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send * out the FIFO, 0 when removing from the FIFO. */ static inv_error_t inv_construct3_fifo(unsigned char *regs, uint_fast16_t elements, uint_fast16_t accuracy, uint_fast8_t refOffset, unsigned short key, uint_fast8_t configOffset) { int_fast8_t kk; inv_error_t result; elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3); accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset); if (accuracy & INV_16_BIT) { regs[0] = DINAF8 + 2; } fifo_obj.data_config[configOffset] = elements | accuracy; for (kk = 0; kk < 3; ++kk) { if ((elements & 1) == 0) regs[kk + 1] = DINAA0 + 3; elements >>= 1; } result = inv_set_mpu_memory(key, 4, regs); return result; } /** * @internal * Puts footer on FIFO data. */ static inv_error_t inv_set_footer(void) { unsigned char regs = DINA30; uint_fast8_t tmp_count; int_fast8_t i, j; int offset; int result; int *fifo_offsets_ptr = fifo_obj.offsets; fifo_obj.fifo_packet_size = 0; for (i = 0; i < NUMFIFOELEMENTS; i++) { tmp_count = 0; offset = fifo_base_offset[i]; for (j = 0; j < 8; j++) { if ((fifo_obj.data_config[i] >> j) & 0x0001) { #ifndef BIG_ENDIAN struct mldl_cfg *mldl_cfg = inv_get_dl_config(); int accel_endian = mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->endian; // Special Case for Byte Ordering on Accel Data if ((i == CONFIG_RAW_DATA) && (j > 2)) { tmp_count += 2; switch (accel_endian) { case EXT_SLAVE_BIG_ENDIAN: *fifo_offsets_ptr++ = offset + 3; *fifo_offsets_ptr++ = offset + 2; break; case EXT_SLAVE_LITTLE_ENDIAN: *fifo_offsets_ptr++ = offset + 2; *fifo_offsets_ptr++ = offset + 3; break; case EXT_SLAVE_FS8_BIG_ENDIAN: if (j == 3) { // Throw this byte away *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; *fifo_offsets_ptr++ = offset + 3; } else if (j == 4) { *fifo_offsets_ptr++ = offset + 3; *fifo_offsets_ptr++ = offset + 7; } else { // Throw these byte away *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; } break; case EXT_SLAVE_FS16_BIG_ENDIAN: if (j == 3) { // Throw this byte away *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; *fifo_offsets_ptr++ = offset + 3; } else if (j == 4) { *fifo_offsets_ptr++ = offset - 2; *fifo_offsets_ptr++ = offset + 3; } else { *fifo_offsets_ptr++ = offset - 2; *fifo_offsets_ptr++ = offset + 3; } break; default: return INV_ERROR; // Bad value on ordering } } else { tmp_count += 2; *fifo_offsets_ptr++ = offset + 3; *fifo_offsets_ptr++ = offset + 2; if (fifo_obj.data_config[i] & INV_32_BIT) { *fifo_offsets_ptr++ = offset + 1; *fifo_offsets_ptr++ = offset; tmp_count += 2; } } #else // Big Endian Platform // Special Case for Byte Ordering on Accel Data if ((i == CONFIG_RAW_DATA) && (j > 2)) { tmp_count += 2; switch (inv_get_dl_config()-> slave[EXT_SLAVE_TYPE_ACCEL]->endian) { case EXT_SLAVE_BIG_ENDIAN: *fifo_offsets_ptr++ = offset + 2; *fifo_offsets_ptr++ = offset + 3; break; case EXT_SLAVE_LITTLE_ENDIAN: *fifo_offsets_ptr++ = offset + 3; *fifo_offsets_ptr++ = offset + 2; break; case EXT_SLAVE_FS8_BIG_ENDIAN: if (j == 3) { // Throw this byte away *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; *fifo_offsets_ptr++ = offset; } else if (j == 4) { *fifo_offsets_ptr++ = offset; *fifo_offsets_ptr++ = offset + 4; } else { // Throw these bytes away *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; } break; case EXT_SLAVE_FS16_BIG_ENDIAN: if (j == 3) { // Throw this byte away *fifo_offsets_ptr++ = fifo_base_offset[CONFIG_FOOTER]; *fifo_offsets_ptr++ = offset; } else if (j == 4) { *fifo_offsets_ptr++ = offset - 3; *fifo_offsets_ptr++ = offset; } else { *fifo_offsets_ptr++ = offset - 3; *fifo_offsets_ptr++ = offset; } break; default: return INV_ERROR; // Bad value on ordering } } else { tmp_count += 2; *fifo_offsets_ptr++ = offset; *fifo_offsets_ptr++ = offset + 1; if (fifo_obj.data_config[i] & INV_32_BIT) { *fifo_offsets_ptr++ = offset + 2; *fifo_offsets_ptr++ = offset + 3; tmp_count += 2; } } #endif } offset += 4; } fifo_obj.fifo_packet_size += tmp_count; } if (fifo_obj.data_config[CONFIG_FOOTER] == 0 && fifo_obj.fifo_packet_size > 0) { // Add footer result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); if (result) { LOG_RESULT_LOCATION(result); return result; } fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT; fifo_obj.fifo_packet_size += 2; } else if (fifo_obj.data_config[CONFIG_FOOTER] && (fifo_obj.fifo_packet_size == 2)) { // Remove Footer regs = DINAA0 + 3; result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); if (result) { LOG_RESULT_LOCATION(result); return result; } fifo_obj.data_config[CONFIG_FOOTER] = 0; fifo_obj.fifo_packet_size = 0; } return INV_SUCCESS; } inv_error_t inv_decode_quantized_accel(void) { int kk; int fifoRate = inv_get_fifo_rate(); if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1)); kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) { union { unsigned int u10:10; signed int s10:10; } temp; union { uint32_t u32; int32_t s32; } value; value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk]; // unquantize this samples. // They are stored as x * 2^20 + y * 2^10 + z // Z temp.u10 = value.u32 & 0x3ff; value.s32 -= temp.s10; fifo_obj.decoded_accel[kk][2] = temp.s10 * 64; // Y value.s32 = value.s32 / 1024; temp.u10 = value.u32 & 0x3ff; value.s32 -= temp.s10; fifo_obj.decoded_accel[kk][1] = temp.s10 * 64; // X value.s32 = value.s32 / 1024; temp.u10 = value.u32 & 0x3ff; fifo_obj.decoded_accel[kk][0] = temp.s10 * 64; } return INV_SUCCESS; } static inv_error_t inv_state_change_fifo(unsigned char newState) { inv_error_t result = INV_SUCCESS; unsigned char regs[4]; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); /* Don't reset the fifo on a fifo rate change */ if ((mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) && (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) { /* Delay output on restart by 50ms due to warm up time of gyros */ short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1); inv_init_fifo_hardare(); inv_int16_to_big8(delay, regs); result = inv_set_mpu_memory(KEY_D_1_178, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } } if (INV_STATE_DMP_STARTED == newState) { if (inv_dmpkey_supported(KEY_D_1_128)) { double tmp; tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1); if (tmp > 0x40000000L) tmp = 0x40000000L; (void)inv_int32_to_big8((long)tmp, regs); result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_reset_fifo(); if (result) { LOG_RESULT_LOCATION(result); return result; } } } return result; } /** * @internal * @brief get the FIFO packet size * @return the FIFO packet size */ uint_fast16_t inv_get_fifo_packet_size(void) { return fifo_obj.fifo_packet_size; } /** * @brief Initializes all the internal static variables for * the FIFO module. * @note Should be called by the initialization routine such * as inv_dmp_open(). * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_init_fifo_param(void) { inv_error_t result; memset(&fifo_obj, 0, sizeof(struct fifo_obj)); fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity inv_set_linear_accel_filter_coef(0.f); fifo_obj.fifo_rate = 20; fifo_obj.sample_step_size_ms = 100; memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj_t)); result = inv_create_mutex(&fifo_rate_obj.mutex); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_register_state_callback(inv_state_change_fifo); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; } /** * @brief Close the FIFO usage. * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_close_fifo(void) { inv_error_t result; inv_error_t first = INV_SUCCESS; result = inv_unregister_state_callback(inv_state_change_fifo); ERROR_CHECK_FIRST(first, result); result = inv_destroy_mutex(fifo_rate_obj.mutex); ERROR_CHECK_FIRST(first, result); memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj_t)); return first; } /** * Set the gyro source to output to the fifo * * @param source The source. One of * - INV_GYRO_FROM_RAW * - INV_GYRO_FROM_QUATERNION * * @return INV_SUCCESS or non-zero error code; */ inv_error_t inv_set_gyro_data_source(uint_fast8_t source) { if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) { return INV_ERROR_INVALID_PARAMETER; } fifo_obj.gyro_source = source; return INV_SUCCESS; } /** * @brief Reads and processes FIFO data. Also handles callbacks when data is * ready. * @param numPackets * Number of FIFO packets to try to read. You should * use a large number here, such as 100, if you want to read all * the full packets in the FIFO, which is typical operation. * @param processed * The number of FIFO packets processed. This may be incremented * even if high rate processes later fail. * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, int_fast8_t * processed) { int_fast8_t packet; inv_error_t result = INV_SUCCESS; uint_fast16_t read; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); int kk; if (NULL == processed) return INV_ERROR_INVALID_PARAMETER; *processed = 0; if (fifo_obj.fifo_packet_size == 0) return result; // Nothing to read for (packet = 0; packet < numPackets; ++packet) { if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) { unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE]; unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE]; read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size, footer_n_data); MPL_LOGD("inv_read_and_process_fifo: read len = %d\, fifo_packet_size = %d\n", read, (uint_fast16_t) fifo_obj.fifo_packet_size); if (0 == read || read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) { result = inv_get_fifo_status(); if (INV_SUCCESS != result) { memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); } if (read != 0) MPL_LOGE("inv_read_and_process_fifo error: read len = %d, fifo status = %d\n", read, result); return result; } result = inv_process_fifo_packet(buf); if (result) { LOG_RESULT_LOCATION(result); return result; } } else if (inv_accel_present()) { long data[ACCEL_NUM_AXES]; result = inv_get_accel_data(data); if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { return INV_SUCCESS; } if (result) { LOG_RESULT_LOCATION(result); return result; } memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); fifo_obj.cache = 0; for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { fifo_obj.decoded[REF_RAW + 4 + kk] = inv_q30_mult((data[kk] << 16), fifo_scale[REF_RAW + 4 + kk]); fifo_obj.decoded[REF_ACCEL + kk] = inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]); fifo_obj.decoded[REF_ACCEL + kk] -= inv_obj.accel->scaled_bias[kk]; } } // The buffer was processed correctly, so increment even if // other processes fail later, which will return an error *processed = *processed + 1; if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) && fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) { result = inv_decode_quantized_accel(); if (result) { LOG_RESULT_LOCATION(result); return result; } } result = inv_pressure_supervisor(); if (result) { LOG_RESULT_LOCATION(result); return result; } // Callbacks now that we have a buffer of data ready result = inv_run_fifo_rate_processes(); if (result) { LOG_RESULT_LOCATION(result); return result; } } return result; } /** * @brief inv_set_fifo_processed_callback is used to set a processed data callback * function. inv_set_fifo_processed_callback sets a user defined callback * function that triggers when all the decoding has been finished by * the motion processing engines. It is called before other bigger * processing engines to allow lower latency for the user. * * @pre inv_dmp_open() * @ifnot MPL_MF * or inv_open_low_power_pedometer() * or inv_eis_open_dmp() * @endif * and inv_dmp_start() * must NOT have been called. * * @param func A user defined callback function. * * @return INV_SUCCESS if successful, or non-zero error code otherwise. */ inv_error_t inv_set_fifo_processed_callback(void (*func) (void)) { INVENSENSE_FUNC_START; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; fifo_obj.fifo_process_cb = func; return INV_SUCCESS; } /** * @internal * @brief Process data from the dmp read via the fifo. Takes a buffer * filled with bytes read from the DMP FIFO. * Currently expects only the 16 bytes of quaternion data. * Calculates the motion parameters from that data and stores the * results in an internal data structure. * @param[in,out] dmpData Pointer to the buffer containing the fifo data. * @return INV_SUCCESS or error code. **/ inv_error_t inv_process_fifo_packet(const unsigned char *dmpData) { INVENSENSE_FUNC_START; int N, kk; unsigned char *p; p = (unsigned char *)(&fifo_obj.decoded); N = fifo_obj.fifo_packet_size; if (N > sizeof(fifo_obj.decoded)) return INV_ERROR_ASSERTION_FAILURE; memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); for (kk = 0; kk < N; ++kk) { p[fifo_obj.offsets[kk]] = *dmpData++; } // If multiplies are much greater cost than if checks, you could check // to see if fifo_scale is non-zero first, or equal to (1L<<30) for (kk = 0; kk < REF_LAST; ++kk) { fifo_obj.decoded[kk] = inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]); } memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS], &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long)); inv_obj.sys->flags[INV_PROCESSED_DATA_READY] = 1; fifo_obj.cache = 0; return INV_SUCCESS; } /** Converts 16-bit temperature data as read from temperature register * into Celcius scaled by 2^16. */ long inv_decode_temperature(short tempReg) { #if defined CONFIG_MPU_SENSORS_MPU6050A2 // Celcius = 35 + (T + 3048.7)/325.9 return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L); #endif #if defined CONFIG_MPU_SENSORS_MPU6050B1 // Celcius = 35 + (T + 927.4)/360.6 return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L); #endif #if defined CONFIG_MPU_SENSORS_MPU3050 // Celcius = 35 + (T + 13200)/280 return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L); #endif } /** @internal * Returns the temperature in hardware units. The scaling may change from part to part. */ inv_error_t inv_get_temperature_raw(short *data) { if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) { inv_error_t result; unsigned char regs[2]; if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) { MPL_LOGV("fetch temp from registers\n"); fifo_obj.cache |= FIFO_CACHE_TEMPERATURE; result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]); } } *data = (short)fifo_obj.decoded[REF_RAW]; return INV_SUCCESS; } /** * @brief Returns 1-element vector of temperature. It is read from the hardware if it * doesn't exist in the FIFO. * @param[out] data 1-element vector of temperature * @return 0 on success or an error code. */ inv_error_t inv_get_temperature(long *data) { short tr; inv_error_t result; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; result = inv_get_temperature_raw(&tr); if (result) { LOG_RESULT_LOCATION(result); return result; } data[0] = inv_decode_temperature(tr); return INV_SUCCESS; } /** * @brief Get the Decoded Accel Data. * @param data * a buffer to store the quantized data. * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_get_unquantized_accel(long *data) { int ii, kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) return INV_ERROR_FEATURE_NOT_ENABLED; for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk]; } } return INV_SUCCESS; } /** * @brief Get the Quantized Accel data algorithm output from the FIFO. * @param data * a buffer to store the quantized data. * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_get_quantized_accel(long *data) { int ii; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) return INV_ERROR_FEATURE_NOT_ENABLED; for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii]; } return INV_SUCCESS; } /** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO * and it is read from the registers if it was not put into the FIFO. The data is * cached till the next FIFO processing block time. * @param[out] data Length 3, Gyro data */ inv_error_t inv_get_gyro_sensor(long *data) { if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) { inv_error_t result; unsigned char regs[6]; if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) { MPL_LOGV("fetch gyro from registers\n"); fifo_obj.cache |= FIFO_CACHE_GYRO; result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } fifo_obj.decoded[REF_RAW + 1] = (((long)regs[0]) << 24) | (((long)regs[1]) << 16); fifo_obj.decoded[REF_RAW + 2] = (((long)regs[2]) << 24) | (((long)regs[3]) << 16); fifo_obj.decoded[REF_RAW + 3] = (((long)regs[4]) << 24) | (((long)regs[5]) << 16); // Temperature starts at location 0, Gyro at location 1. fifo_obj.decoded[REF_RAW + 1] = inv_q30_mult(fifo_obj.decoded[REF_RAW + 1], fifo_scale[REF_RAW + 1]); fifo_obj.decoded[REF_RAW + 2] = inv_q30_mult(fifo_obj.decoded[REF_RAW + 2], fifo_scale[REF_RAW + 2]); fifo_obj.decoded[REF_RAW + 3] = inv_q30_mult(fifo_obj.decoded[REF_RAW + 3], fifo_scale[REF_RAW + 3]); } data[0] = fifo_obj.decoded[REF_RAW + 1]; data[1] = fifo_obj.decoded[REF_RAW + 2]; data[2] = fifo_obj.decoded[REF_RAW + 3]; } else { long data2[6]; inv_get_gyro_and_accel_sensor(data2); data[0] = data2[0]; data[1] = data2[1]; data[2] = data2[2]; } return INV_SUCCESS; } /** * @brief Returns 6-element vector of gyro and accel data * @param[out] data 6-element vector of gyro and accel data * @return 0 on success or an error code. */ inv_error_t inv_get_gyro_and_accel_sensor(long *data) { int ii; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_RAW_DATA]) return INV_ERROR_FEATURE_NOT_ENABLED; for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) { data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii]; } return INV_SUCCESS; } /** * @brief Returns 3-element vector of external sensor. * @param[out] data * 3-element vector of external sensor * @param[in] size * the size of the buffer. * @return 0 on success or an error code. */ inv_error_t inv_get_external_sensor_data(long *data, int size) { #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 int ii; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL]) return INV_ERROR_FEATURE_NOT_ENABLED; for (ii = 0; ii < size && ii < 6; ii++) { data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii]; } return INV_SUCCESS; #else memset(data, 0, COMPASS_NUM_AXES * sizeof(long)); return INV_ERROR_FEATURE_NOT_IMPLEMENTED; #endif } /** * Sends accelerometer data to the FIFO. * * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together * for a subset. * * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. Set to zero to remove it from the FIFO. */ inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 }; inv_error_t result; int kk; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL, KEY_CFG_12, CONFIG_ACCEL); if (result) { LOG_RESULT_LOCATION(result); return result; } for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel->sens; } return inv_set_footer(); } /** * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits. * * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd * together for a subset. * * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. Set to zero to remove it from the FIFO. */ inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; int_fast8_t kk; inv_error_t result; unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4); accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA); if (accuracy & INV_16_BIT) { regs[0] = DINAF8 + 2; } fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy; for (kk = 0; kk < 4; ++kk) { if ((elements & 1) == 0) regs[kk + 1] = DINAA0 + 3; elements >>= 1; } result = inv_set_mpu_memory(KEY_CFG_1, 5, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); } /** * Adds a rolling counter to the fifo packet. When used with the footer * the data comes out the first time: * * @code * ... * @endcode * for every other packet it is * * @code * ... * @endcode * * This allows for scanning of the fifo for packets * * @return INV_SUCCESS or error code */ inv_error_t inv_send_packet_number(uint_fast16_t accuracy) { INVENSENSE_FUNC_START; inv_error_t result; unsigned char regs; uint_fast16_t elements; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1); if (elements & 1) { regs = DINA28; fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = INV_ELEMENT_1 | INV_16_BIT; } else { regs = DINAF8 + 3; fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0; } result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); } /** * @brief Send the computed gravity vectors into the FIFO. * The gravity vectors can be retrieved from the FIFO via * inv_get_gravity(), to have the gravitation vector expressed * in coordinates relative to the body. * * Gravity is a derived vector derived from the quaternion. * @param elements * the gravitation vectors components bitmask. * To send all compoents use INV_ALL. * @param accuracy * The number of bits the gravitation vector is expressed * into. * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. * Set to zero to remove it from the FIFO. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; inv_error_t result; result = inv_send_quaternion(accuracy); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); } /** Sends gyro data to the FIFO. Gyro data is a 3 length vector * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together * for a subset. * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. Set to zero to remove it from the FIFO. */ inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 }; inv_error_t result; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) { regs[0] = DINA90 + 5; result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } regs[0] = DINAF8 + 1; regs[1] = DINA20; regs[2] = DINA28; regs[3] = DINA30; } else { regs[0] = DINA90 + 10; result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } regs[0] = DINAF8 + 1; regs[1] = DINA28; regs[2] = DINA30; regs[3] = DINA38; } result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS, KEY_CFG_9, CONFIG_GYROS); return inv_set_footer(); } /** Sends linear accelerometer data to the FIFO. * * Linear accelerometer data is a 3 length vector of 32-bits. It is the * acceleration in the body frame with gravity removed. * * * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together * for a subset. * * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. Set to zero to remove it from the FIFO. */ inv_error_t inv_send_linear_accel(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; inv_error_t result; unsigned char state = inv_get_state(); if (state < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; result = inv_send_gravity(elements, accuracy); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_send_accel(elements, accuracy); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); } /** Sends linear world accelerometer data to the FIFO. Linear world * accelerometer data is a 3 length vector of 32-bits. It is the acceleration * in the world frame with gravity removed. Should be called once after * inv_dmp_open() and before inv_dmp_start(). * * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together * for a subset. * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. */ inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; inv_error_t result; result = inv_send_linear_accel(INV_ALL, accuracy); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_send_quaternion(accuracy); return inv_set_footer(); } /** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 * bit data. */ inv_error_t inv_send_quaternion(uint_fast16_t accuracy) { INVENSENSE_FUNC_START; unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; uint_fast16_t elements, kk; inv_error_t result; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4); accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT); if (accuracy & INV_16_BIT) { regs[0] = DINAF8 + 2; } fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy; for (kk = 0; kk < 4; ++kk) { if ((elements & 1) == 0) regs[kk + 1] = DINAA0 + 3; elements >>= 1; } result = inv_set_mpu_memory(KEY_CFG_8, 5, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); } /** Sends raw data to the FIFO. * Should be called once after inv_dmp_open() and before inv_dmp_start(). * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together * for a subset. The first element is temperature, the next 3 are gyro data, * and the last 3 accel data. * @param accuracy * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off. * @return 0 if successful, a non-zero error code otherwise. */ inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy) { int result; #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 unsigned char regs[10] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3 }; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); char compasspresent = inv_compass_present(); if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (accuracy) accuracy = INV_16_BIT; elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); if (elements & 1) fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; else fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; if (elements & 0x7e) fifo_obj.data_config[CONFIG_RAW_DATA] = (0x3f & (elements >> 1)) | INV_16_BIT; else fifo_obj.data_config[CONFIG_RAW_DATA] = 0; if (elements & INV_ELEMENT_1) { regs[0] = DINACA; } if (elements & INV_ELEMENT_2) { regs[1] = DINBC4; } if (elements & INV_ELEMENT_3) { regs[2] = DINACC; } if (elements & INV_ELEMENT_4) { regs[3] = DINBC6; } if ((elements & INV_ELEMENT_5) || (elements & INV_ELEMENT_6) || (elements & INV_ELEMENT_7)) { if (ACCEL_ID_MPU6050 == mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id) { regs[4] = DINBC0; regs[5] = DINAC8; regs[6] = DINBC2; } else { if (compasspresent) { regs[4] = DINA80 + 1; regs[5] = DINBC4 + 1; regs[6] = DINACD; regs[7] = DINBC6 + 1; regs[8] = DINA80; } else { regs[4] = DINACF; regs[5] = DINA80 + 1; regs[6] = DINBC0 + 1; regs[7] = DINAC9; regs[8] = DINA80; } } } result = inv_set_mpu_memory(KEY_CFG_15, 10, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); #else INVENSENSE_FUNC_START; unsigned char regs[4] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3 }; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (accuracy) accuracy = INV_16_BIT; elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); if (elements & 0x03) { elements |= 0x03; regs[0] = DINA20; } if (elements & 0x0C) { elements |= 0x0C; regs[1] = DINA28; } if (elements & 0x30) { elements |= 0x30; regs[2] = DINA30; } if (elements & 0x40) { elements |= 0xC0; regs[3] = DINA38; } result = inv_set_mpu_memory(KEY_CFG_15, 4, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } if (elements & 0x01) fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; else fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; if (elements & 0xfe) fifo_obj.data_config[CONFIG_RAW_DATA] = (0x7f & (elements >> 1)) | INV_16_BIT; else fifo_obj.data_config[CONFIG_RAW_DATA] = 0; return inv_set_footer(); #endif } /** Sends raw external data to the FIFO. * Should be called once after inv_dmp_open() and before inv_dmp_start(). * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together * for a subset. * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data. * Sending and Stop sending are reference counted, so data actually * stops when the reference reaches zero. */ inv_error_t inv_send_external_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy) { #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 int result; unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, DINAA0 + 3 }; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (accuracy) accuracy = INV_16_BIT; elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6); if (elements) fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT; else fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0; if (elements & INV_ELEMENT_1) { regs[0] = DINBC2; } if (elements & INV_ELEMENT_2) { regs[1] = DINACA; } if (elements & INV_ELEMENT_3) { regs[2] = DINBC4; } if (elements & INV_ELEMENT_4) { regs[3] = DINBC0; } if (elements & INV_ELEMENT_5) { regs[4] = DINAC8; } if (elements & INV_ELEMENT_6) { regs[5] = DINACC; } result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); #else return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported #endif } /** * @brief Send the Quantized Acceleromter data into the FIFO. The data can be * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel(). * * To be useful this should be set to fifo_rate + 1 if less than * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work. * * @param elements * the components bitmask. * To send all compoents use INV_ALL. * * @param accuracy * Use INV_32_BIT for 32-bit data or INV_16_BIT for * 16-bit data. * Set to zero to remove it from the FIFO. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_send_quantized_accel(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; unsigned char regs2[4] = { DINA20, DINA28, DINA30, DINA38 }; inv_error_t result; int_fast8_t kk; int_fast8_t ii; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8); if (elements) { fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT; } else { fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0; } for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) { fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0; for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { fifo_obj.decoded_accel[kk][ii] = 0; } } for (kk = 0; kk < 4; ++kk) { if ((elements & 1) == 0) regs[kk + 1] = DINAA0 + 3; elements >>= 1; } result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } for (kk = 0; kk < 4; ++kk) { if ((elements & 1) == 0) regs2[kk] = DINAA0 + 3; elements >>= 1; } result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2); if (result) { LOG_RESULT_LOCATION(result); return result; } return inv_set_footer(); } inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy) { INVENSENSE_FUNC_START; int_fast8_t kk; unsigned char regs[3] = { DINA28, DINA30, DINA38 }; inv_error_t result; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (accuracy) { accuracy = INV_32_BIT; } elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3); accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS); fifo_obj.data_config[CONFIG_EIS] = elements | accuracy; for (kk = 0; kk < 3; ++kk) { if ((elements & 1) == 0) regs[kk] = DINAA0 + 7; elements >>= 1; } result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs); return inv_set_footer(); } /** * @brief Returns 3-element vector of accelerometer data in body frame. * * @param[out] data 3-element vector of accelerometer data in body frame. * One gee = 2^16. * @return 0 on success or an error code. */ inv_error_t inv_get_accel(long *data) { int kk; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if ((!fifo_obj.data_config[CONFIG_ACCEL] && (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) || (!(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) && !inv_accel_present())) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { data[kk] = fifo_obj.decoded[REF_ACCEL + kk]; } return INV_SUCCESS; } /** * @brief Returns 4-element quaternion vector derived from 6-axis or * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is * gyros, accel and compass. * * @param[out] data 4-element quaternion vector. One is scaled to 2^30. * @return 0 on success or an error code. */ inv_error_t inv_get_quaternion(long *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_QUAT]) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = 0; kk < 4; ++kk) { data[kk] = fifo_obj.decoded[REF_QUATERNION + kk]; } return INV_SUCCESS; } /** * @brief Returns 4-element quaternion vector derived from 6 * axis sensors (gyros and accels). * @param[out] data * 4-element quaternion vector. One is scaled to 2^30. * @return 0 on success or an error code. */ inv_error_t inv_get_6axis_quaternion(long *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_QUAT]) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = 0; kk < 4; ++kk) { data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk]; } return INV_SUCCESS; } inv_error_t inv_get_relative_quaternion(long *data) { if (IS_INV_ADVFEATURES_ENABLED(inv_obj)) { if (data == NULL) return INV_ERROR; data[0] = inv_obj.adv_fusion->relative_quat[0]; data[1] = inv_obj.adv_fusion->relative_quat[1]; data[2] = inv_obj.adv_fusion->relative_quat[2]; data[3] = inv_obj.adv_fusion->relative_quat[3]; return INV_SUCCESS; } else { return INV_ERROR_FEATURE_NOT_ENABLED; } } /** * @brief Returns 3-element vector of gyro data in body frame. * @param[out] data * 3-element vector of gyro data in body frame * with gravity removed. One degree per second = 2^16. * @return 0 on success or an error code. */ inv_error_t inv_get_gyro(long *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (fifo_obj.data_config[CONFIG_GYROS]) { for (kk = 0; kk < 3; ++kk) { data[kk] = fifo_obj.decoded[REF_GYROS + kk]; } return INV_SUCCESS; } else { return INV_ERROR_FEATURE_NOT_ENABLED; } } /** * @brief Get the 3-element gravity vector from the FIFO expressed * in coordinates relative to the body frame. * @param data * 3-element vector of gravity in body frame. * @return 0 on success or an error code. */ inv_error_t inv_get_gravity(long *data) { long quat[4]; int ii; inv_error_t result; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_QUAT]) return INV_ERROR_FEATURE_NOT_ENABLED; if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) { fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY; // Compute it from Quaternion result = inv_get_quaternion(quat); if (result) { LOG_RESULT_LOCATION(result); return result; } data[0] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); data[1] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); data[2] = (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) - 1073741824L; for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { data[ii] >>= 14; fifo_obj.gravity_cache[ii] = data[ii]; } } else { data[0] = fifo_obj.gravity_cache[0]; data[1] = fifo_obj.gravity_cache[1]; data[2] = fifo_obj.gravity_cache[2]; } return INV_SUCCESS; } /** * @brief Sets the filter coefficent used for computing the acceleration * bias which is used to compute linear acceleration. * @param[in] coef Fitler coefficient. 0. means no filter, a small number means * a small cutoff frequency with an increasing number meaning * an increasing cutoff frequency. */ inv_error_t inv_set_linear_accel_filter_coef(float coef) { fifo_obj.acc_filter_coef = coef; return INV_SUCCESS; } /** * @brief Returns 3-element vector of accelerometer data in body frame * with gravity removed. * @param[out] data 3-element vector of accelerometer data in body frame * with gravity removed. One g = 2^16. * @return 0 on success or an error code. data unchanged on error. */ inv_error_t inv_get_linear_accel(long *data) { int kk; long grav[3]; long la[3]; inv_error_t result; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; result = inv_get_gravity(grav); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_get_accel(la); if (result) { LOG_RESULT_LOCATION(result); return result; } if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) { fifo_obj.cache |= FIFO_CACHE_ACC_BIAS; for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { long x; x = la[kk] - grav[kk]; fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef + fifo_obj.acc_bias_filt[kk] * (1.f - fifo_obj.acc_filter_coef)); data[kk] = x - fifo_obj.acc_bias_filt[kk]; } } else { for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk]; } } return INV_SUCCESS; } /** * @brief Returns 3-element vector of accelerometer data in world frame * with gravity removed. * @param[out] data 3-element vector of accelerometer data in world frame * with gravity removed. One g = 2^16. * @return 0 on success or an error code. */ inv_error_t inv_get_linear_accel_in_world(long *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) { long wtemp[4], qi[4], wtemp2[4]; wtemp[0] = 0; inv_get_linear_accel(&wtemp[1]); inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2); inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi); inv_q_mult(wtemp2, qi, wtemp); for (kk = 0; kk < 3; ++kk) { data[kk] = wtemp[kk + 1]; } return INV_SUCCESS; } else { return INV_ERROR_FEATURE_NOT_ENABLED; } } /** * @brief Returns 4-element vector of control data. * @param[out] data 4-element vector of control data. * @return 0 for succes or an error code. */ inv_error_t inv_get_cntrl_data(long *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_CONTROL_DATA]) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = 0; kk < 4; ++kk) { data[kk] = fifo_obj.decoded[REF_CONTROL + kk]; } return INV_SUCCESS; } /** * @brief Returns 3-element vector of EIS shfit data * @param[out] data 3-element vector of EIS shift data. * @return 0 for succes or an error code. */ inv_error_t inv_get_eis(long *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_EIS]) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = 0; kk < 3; ++kk) { data[kk] = fifo_obj.decoded[REF_EIS + kk]; } return INV_SUCCESS; } /** * @brief Returns 3-element vector of accelerometer data in body frame. * @param[out] data 3-element vector of accelerometer data in body frame in g's. * @return 0 for success or an error code. */ inv_error_t inv_get_accel_float(float *data) { long lData[3]; int kk; int result; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; result = inv_get_accel(lData); if (result) { LOG_RESULT_LOCATION(result); return result; } for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { data[kk] = lData[kk] / 65536.0f; } return INV_SUCCESS; } /** * @brief Returns 4-element quaternion vector. * @param[out] data 4-element quaternion vector. * @return 0 on success, an error code otherwise. */ inv_error_t inv_get_quaternion_float(float *data) { int kk; if (data == NULL) return INV_ERROR_INVALID_PARAMETER; if (!fifo_obj.data_config[CONFIG_QUAT]) return INV_ERROR_FEATURE_NOT_ENABLED; for (kk = 0; kk < 4; ++kk) { data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f; } return INV_SUCCESS; } /** * @brief Command the MPU to put data in the FIFO at a particular rate. * * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For * example if the MPU is running at 200Hz the following values apply: * * * * * * * * * *
fifoRateDMP Sample RateFIFO update frequency
0200Hz200Hz
1200Hz100Hz
2200Hz50Hz
4200Hz40Hz
9200Hz20Hz
19200Hz10Hz
* * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED) * then inv_run_state_callbacks() will be called to allow features * that depend upon fundamental constants to be updated. * * @pre inv_dmp_open() * @ifnot MPL_MF * or inv_open_low_power_pedometer() * or inv_eis_open_dmp() * @endif * and inv_dmp_start() * must NOT have been called. * * @param fifoRate Divider value - 1. Output rate is * (DMP Sample Rate) / (fifoRate + 1). * * @return INV_SUCCESS if successful, ML error code on any failure. */ inv_error_t inv_set_fifo_rate(unsigned short fifoRate) { INVENSENSE_FUNC_START; unsigned char regs[2]; unsigned char state; inv_error_t result = INV_SUCCESS; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); state = inv_get_state(); if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED) return INV_ERROR_SM_IMPROPER_STATE; if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) { fifo_obj.fifo_rate = fifoRate; regs[0] = (unsigned char)((fifoRate >> 8) & 0xff); regs[1] = (unsigned char)(fifoRate & 0xff); result = inv_set_mpu_memory(KEY_D_0_22, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } fifo_obj.sample_step_size_ms = (unsigned short)(((long)fifoRate + 1) * (inv_mpu_get_sampling_period_us (mldl_cfg->mpu_gyro_cfg)) / 1000L); if (state == INV_STATE_DMP_STARTED) result = inv_run_state_callbacks(state); } else if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) { struct ext_slave_config config; long data; config.key = MPU_SLAVE_CONFIG_ODR_RESUME; config.len = sizeof(long); config.apply = (state == INV_STATE_DMP_STARTED); config.data = &data; data = (1000 * inv_mpu_get_sampling_rate_hz( mldl_cfg->mpu_gyro_cfg)) / (fifoRate + 1); /* Ask for the same frequency */ result = inv_mpu_config_accel(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_mpu_get_accel_config(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } MPL_LOGI("Actual ODR: %ld mHz\n", data); /* Record the actual frequency granted odr is in mHz */ fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data); } return result; } /** * @brief Retrieve the current FIFO update divider - 1. * See inv_set_fifo_rate() for how this value is used. * * The fifo rate when there is no fifo is the equivilent divider when * derived from the value set by SetSampleSteSizeMs() * * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error. */ unsigned short inv_get_fifo_rate(void) { struct mldl_cfg *mldl_cfg = inv_get_dl_config(); if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) return fifo_obj.fifo_rate; else return (unsigned short)((1000 * fifo_obj.sample_step_size_ms) / (inv_mpu_get_sampling_period_us( mldl_cfg->mpu_gyro_cfg))) - 1; } /** * @brief Returns the step size for quaternion type data. * * Typically the data rate for each FIFO packet. When the gryos are sleeping * this value will return the last value set by SetSampleStepSizeMs() * * @return step size for quaternion type data */ int_fast16_t inv_get_sample_step_size_ms(void) { struct mldl_cfg *mldl_cfg = inv_get_dl_config(); if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) return (fifo_obj.fifo_rate + 1) * (inv_mpu_get_sampling_period_us(mldl_cfg->mpu_gyro_cfg) / 1000); else return fifo_obj.sample_step_size_ms; } /** * @brief Returns the step size for quaternion type data. * * Typically the data rate for each FIFO packet. When the gryos are sleeping * this value will return the last value set by SetSampleStepSizeMs() * * @return step size for quaternion type data */ int_fast16_t inv_get_sample_frequency(void) { struct mldl_cfg *mldl_cfg = inv_get_dl_config(); if (mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR) return (inv_mpu_get_sampling_rate_hz(mldl_cfg->mpu_gyro_cfg) / (fifo_obj.fifo_rate + 1)); else return (1000 / fifo_obj.sample_step_size_ms); } /** * @brief The gyro data magnitude squared : * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. * @return the computed magnitude squared output of the gyroscope. */ unsigned long inv_get_gyro_sum_of_sqr(void) { unsigned long gmag = 0; long temp; int kk; for (kk = 0; kk < 3; ++kk) { temp = fifo_obj.decoded[REF_GYROS + kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); gmag += temp * temp; } return gmag; } /** * @brief The gyro data magnitude squared: * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT. * @return the computed magnitude squared output of the accelerometer. */ unsigned long inv_accel_sum_of_sqr(void) { unsigned long amag = 0; long temp; int kk; long accel[3]; inv_error_t result; result = inv_get_accel(accel); if (INV_SUCCESS != result) { return 0; } for (kk = 0; kk < 3; ++kk) { temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2)); amag += temp * temp; } return amag; } /** * @internal */ void inv_override_quaternion(long *q) { memcpy(&fifo_obj.decoded[REF_QUATERNION], q, 4*sizeof(long)); } /** * @internal * @brief This registers a function to be called for each set of * gyro/quaternion/rotation matrix/etc output. * @param[in] func The callback function to register * @param[in] priority The unique priority number of the callback. Lower numbers * are called first. * @return error code. */ inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority) { INVENSENSE_FUNC_START; inv_error_t result; int kk, nn; result = inv_lock_mutex(fifo_rate_obj.mutex); if (INV_SUCCESS != result) { return result; } // Make sure we haven't registered this function already // Or used the same priority for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { if ((fifo_rate_obj.fifo_process_cb[kk] == func) || (fifo_rate_obj.priority[kk] == priority)) { inv_unlock_mutex(fifo_rate_obj.mutex); return INV_ERROR_INVALID_PARAMETER; } } // Make sure we have not filled up our number of allowable callbacks if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) { kk = 0; if (fifo_rate_obj.num_cb != 0) { // set kk to be where this new callback goes in the array while ((kk < fifo_rate_obj.num_cb) && (fifo_rate_obj.priority[kk] < priority)) { kk++; } if (kk != fifo_rate_obj.num_cb) { // We need to move the others for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) { fifo_rate_obj.fifo_process_cb[nn] = fifo_rate_obj.fifo_process_cb[nn - 1]; fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1]; } } } // Add new callback fifo_rate_obj.fifo_process_cb[kk] = func; fifo_rate_obj.priority[kk] = priority; fifo_rate_obj.num_cb++; } else { result = INV_ERROR_MEMORY_EXAUSTED; } inv_unlock_mutex(fifo_rate_obj.mutex); return result; } /** * @internal * @brief This unregisters a function to be called for each set of * gyro/quaternion/rotation matrix/etc output. * @return error code. */ inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func) { INVENSENSE_FUNC_START; int kk, jj; inv_error_t result; result = inv_lock_mutex(fifo_rate_obj.mutex); if (INV_SUCCESS != result) { return result; } // Make sure we haven't registered this function already result = INV_ERROR_INVALID_PARAMETER; for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { if (fifo_rate_obj.fifo_process_cb[kk] == func) { for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) { fifo_rate_obj.fifo_process_cb[jj - 1] = fifo_rate_obj.fifo_process_cb[jj]; fifo_rate_obj.priority[jj - 1] = fifo_rate_obj.priority[jj]; } fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL; fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0; fifo_rate_obj.num_cb--; result = INV_SUCCESS; break; } } inv_unlock_mutex(fifo_rate_obj.mutex); return result; } inv_error_t inv_run_fifo_rate_processes(void) { int kk; inv_error_t result, result2; result = inv_lock_mutex(fifo_rate_obj.mutex); if (INV_SUCCESS != result) { MPL_LOGE("MLOsLockMutex returned %d\n", result); return result; } // User callbacks take priority over the fifo_process_cb callback if (fifo_obj.fifo_process_cb) fifo_obj.fifo_process_cb(); for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { if (fifo_rate_obj.fifo_process_cb[kk]) { result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj); if (result == INV_SUCCESS) result = result2; } } inv_unlock_mutex(fifo_rate_obj.mutex); return result; } /** * @} */