1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/******************************************************************************* 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * $Id: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $ 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *******************************************************************************/ 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup MLFIFO 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Motion Library - FIFO Driver. 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The FIFO API Interface. 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file mlFIFO.c 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief FIFO Interface. 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mpu.h" 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu6050a2.h" 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#elif defined CONFIG_MPU_SENSORS_MPU6050B1 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu6050b1.h" 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#elif defined CONFIG_MPU_SENSORS_MPU3050 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall# include "mpu3050.h" 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFOHW.h" 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpKey.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc.h" 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlstates.h" 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsupervisor.h" 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlmath.h" 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "accel.h" 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-fifo" 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FIFO_DEBUG 0 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_QUATERNION (0) 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_GYROS (REF_QUATERNION + 4) 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_CONTROL (REF_GYROS + 3) 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_RAW (REF_CONTROL + 4) 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_RAW_EXTERNAL (REF_RAW + 8) 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_ACCEL (REF_RAW_EXTERNAL + 6) 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_QUANT_ACCEL (REF_ACCEL + 3) 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_EIS (REF_QUATERNION_6AXIS + 4) 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_DMP_PACKET (REF_EIS + 3) 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_GARBAGE (REF_DMP_PACKET + 1) 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define REF_LAST (REF_GARBAGE + 1) 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgralllong fifo_scale[REF_LAST] = { 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2) 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1501974482L, 1501974482L, 1501974482L, // Gyro 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 14), // Temperature 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 14), (1L << 14), (1L << 14), // Raw Gyro 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 14), (1L << 14), (1L << 14), // Raw External 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 14), (1L << 14), (1L << 14), // Raw External 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 16), (1L << 16), (1L << 16), // Accel 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), (1L << 30), (1L << 30), // EIS 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), // Packet 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1L << 30), // Garbage 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall// The scale factors for tap need to match the number in fifo_scale above. 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall// fifo_base_offset below may also need to be changed if this is not 8 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if INV_MAX_NUM_ACCEL_SAMPLES != 8 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_QUAT (0) 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_GYROS (CONFIG_QUAT + 1) 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1) 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1) 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1) 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1) 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1) 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1) 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1) 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1) 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1) 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define NUMFIFOELEMENTS (CONFIG_FOOTER + 1) 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallconst int fifo_base_offset[NUMFIFOELEMENTS] = { 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_QUATERNION * 4, 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_GYROS * 4, 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_CONTROL * 4, 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_RAW * 4, 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_RAW * 4 + 4, 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_RAW_EXTERNAL * 4, 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_ACCEL * 4, 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_QUANT_ACCEL * 4, 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_EIS * 4, 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_DMP_PACKET * 4, 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall REF_GARBAGE * 4 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct fifo_obj { 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void (*fifo_process_cb) (void); 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long decoded[REF_LAST]; 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES]; 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int offsets[REF_LAST * 4]; 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int cache; 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t gyro_source; 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short fifo_rate; 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short sample_step_size_ms; 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t fifo_packet_size; 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t data_config[NUMFIFOELEMENTS]; 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char reference_count[REF_LAST]; 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long acc_bias_filt[3]; 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float acc_filter_coef; 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long gravity_cache[3]; 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic struct fifo_obj fifo_obj; 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FIFO_CACHE_TEMPERATURE 1 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FIFO_CACHE_GYRO 2 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FIFO_CACHE_GRAVITY_BODY 4 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FIFO_CACHE_ACC_BIAS 8 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct fifo_rate_obj { 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // These describe callbacks happening everytime a FIFO block is processed 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t num_cb; 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall HANDLE mutex; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES]; 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int priority[MAX_HIGH_RATE_PROCESSES]; 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct fifo_rate_obj fifo_rate_obj; 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * accuracy if needed. 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements, 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t accuracy, 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t configOffset) 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements) { 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!accuracy) 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = fifo_obj.data_config[configOffset]; 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (accuracy & INV_16_BIT) 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.data_config[configOffset] & INV_32_BIT)) 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_32_BIT; // 32-bits takes priority 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_16_BIT; 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_32_BIT; 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = 0; 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return accuracy; 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the reference counts are subtracted, otherwise they are incremented for each 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit set in element. The value returned are the elements that should be sent 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * out as data through the FIFO. 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements, 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t increment, 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t refOffset, 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t len) 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t kk; 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (increment == 0) { 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < len; ++kk) { 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall && (fifo_obj.reference_count[kk + refOffset] > 0)) { 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.reference_count[kk + refOffset]--; 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < len; ++kk) { 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 1) 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.reference_count[kk + refOffset]++; 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = 0; 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < len; ++kk) { 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.reference_count[kk + refOffset] > 0) 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements |= (1 << kk); 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return elements; 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * out the FIFO, 0 when removing from the FIFO. 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t inv_construct3_fifo(unsigned char *regs, 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t elements, 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t accuracy, 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t refOffset, 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short key, 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t configOffset) 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t kk; 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3); 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset); 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy & INV_16_BIT) { 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAF8 + 2; 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[configOffset] = elements | accuracy; 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) == 0) 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk + 1] = DINAA0 + 3; 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(key, 4, regs); 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Puts footer on FIFO data. 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t inv_set_footer(void) 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs = DINA30; 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t tmp_count; 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t i, j; 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int offset; 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int result; 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int *fifo_offsets_ptr = fifo_obj.offsets; 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_packet_size = 0; 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < NUMFIFOELEMENTS; i++) { 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count = 0; 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall offset = fifo_base_offset[i]; 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 8; j++) { 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.data_config[i] >> j) & 0x0001) { 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifndef BIG_ENDIAN 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Special Case for Byte Ordering on Accel Data 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((i == CONFIG_RAW_DATA) && (j > 2)) { 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count += 2; 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (inv_get_dl_config()->accel->endian) { 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_BIG_ENDIAN: 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 2; 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_LITTLE_ENDIAN: 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 2; 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_FS8_BIG_ENDIAN: 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (j == 3) { 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Throw this byte away 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (j == 4) { 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 7; 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Throw these byte away 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_FS16_BIG_ENDIAN: 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (j == 3) { 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Throw this byte away 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (j == 4) { 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset - 2; 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset - 2; 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; // Bad value on ordering 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count += 2; 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 2; 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.data_config[i] & INV_32_BIT) { 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 1; 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count += 2; 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Big Endian Platform 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Special Case for Byte Ordering on Accel Data 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((i == CONFIG_RAW_DATA) && (j > 2)) { 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count += 2; 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (inv_get_dl_config()->accel->endian) { 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_BIG_ENDIAN: 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 2; 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_LITTLE_ENDIAN: 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 2; 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_FS8_BIG_ENDIAN: 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (j == 3) { 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Throw this byte away 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (j == 4) { 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 4; 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Throw these bytes away 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_FS16_BIG_ENDIAN: 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (j == 3) { 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Throw this byte away 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_base_offset[CONFIG_FOOTER]; 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (j == 4) { 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset - 3; 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset - 3; 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; // Bad value on ordering 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count += 2; 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset; 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 1; 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.data_config[i] & INV_32_BIT) { 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 2; 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *fifo_offsets_ptr++ = offset + 3; 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp_count += 2; 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall offset += 4; 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_packet_size += tmp_count; 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.data_config[CONFIG_FOOTER] == 0 && 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_packet_size > 0) { 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Add footer 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT; 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_packet_size += 2; 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (fifo_obj.data_config[CONFIG_FOOTER] && 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (fifo_obj.fifo_packet_size == 2)) { 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Remove Footer 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs = DINAA0 + 3; 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_FOOTER] = 0; 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_packet_size = 0; 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_decode_quantized_accel(void) 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int fifoRate = inv_get_fifo_rate(); 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1)); 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) { 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall union { 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int u10:10; 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall signed int s10:10; 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } temp; 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall union { 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint32_t u32; 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int32_t s32; 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } value; 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk]; 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // unquantize this samples. 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // They are stored as x * 2^20 + y * 2^10 + z 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Z 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp.u10 = value.u32 & 0x3ff; 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall value.s32 -= temp.s10; 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded_accel[kk][2] = temp.s10 * 64; 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Y 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall value.s32 = value.s32 / 1024; 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp.u10 = value.u32 & 0x3ff; 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall value.s32 -= temp.s10; 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded_accel[kk][1] = temp.s10 * 64; 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // X 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall value.s32 = value.s32 / 1024; 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp.u10 = value.u32 & 0x3ff; 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded_accel[kk][0] = temp.s10 * 64; 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t inv_state_change_fifo(unsigned char newState) 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4]; 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Don't reset the fifo on a fifo rate change */ 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) { 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Delay output on restart by 50ms due to warm up time of gyros */ 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1); 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_init_fifo_hardare(); 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_int16_to_big8(delay, regs); 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_178, 2, regs); 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_STATE_DMP_STARTED == newState) { 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmpkey_supported(KEY_D_1_128)) { 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double tmp; 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1); 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 0x40000000L) 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 0x40000000L; 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (void)inv_int32_to_big8((long)tmp, regs); 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs); 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_reset_fifo(); 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief get the FIFO packet size 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return the FIFO packet size 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgralluint_fast16_t inv_get_fifo_packet_size(void) 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return fifo_obj.fifo_packet_size; 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Initializes all the internal static variables for 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the FIFO module. 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @note Should be called by the initialization routine such 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * as inv_dmp_open(). 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_init_fifo_param(void) 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&fifo_obj, 0, sizeof(struct fifo_obj)); 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_linear_accel_filter_coef(0.f); 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_rate = 20; 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.sample_step_size_ms = 100; 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_create_mutex(&fifo_rate_obj.mutex); 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_register_state_callback(inv_state_change_fifo); 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Close the FIFO usage. 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_close_fifo(void) 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t first = INV_SUCCESS; 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_unregister_state_callback(inv_state_change_fifo); 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ERROR_CHECK_FIRST(first, result); 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_destroy_mutex(fifo_rate_obj.mutex); 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ERROR_CHECK_FIRST(first, result); 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return first; 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Set the gyro source to output to the fifo 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param source The source. One of 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_GYRO_FROM_RAW 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_GYRO_FROM_QUATERNION 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code; 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_gyro_data_source(uint_fast8_t source) 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) { 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.gyro_source = source; 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Reads and processes FIFO data. Also handles callbacks when data is 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * ready. 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param numPackets 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Number of FIFO packets to try to read. You should 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * use a large number here, such as 100, if you want to read all 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the full packets in the FIFO, which is typical operation. 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param processed 588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The number of FIFO packets processed. This may be incremented 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * even if high rate processes later fail. 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t * processed) 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t packet; 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t read; 598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (NULL == processed) 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *processed = 0; 605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.fifo_packet_size == 0) 606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; // Nothing to read 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (packet = 0; packet < numPackets; ++packet) { 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE]; 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE]; 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size, 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall footer_n_data); 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (0 == read || 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) { 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_fifo_status(); 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_process_fifo_packet(buf); 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (inv_accel_present()) { 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long data[ACCEL_NUM_AXES]; 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_accel_data(data); 631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.cache = 0; 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 4 + kk] = 643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult((data[kk] << 16), 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_scale[REF_RAW + 4 + kk]); 645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_ACCEL + kk] = 646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]); 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_ACCEL + kk] -= 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.scaled_accel_bias[kk]; 649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // The buffer was processed correctly, so increment even if 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // other processes fail later, which will return an error 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *processed = *processed + 1; 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) && 656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) { 657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_decode_quantized_accel(); 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.data_config[CONFIG_QUAT]) { 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_accel_compass_supervisor(); 666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_pressure_supervisor(); 673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Callbacks now that we have a buffer of data ready 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_run_fifo_rate_processes(); 680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_set_fifo_processed_callback is used to set a processed data callback 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * function. inv_set_fifo_processed_callback sets a user defined callback 692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * function that triggers when all the decoding has been finished by 693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the motion processing engines. It is called before other bigger 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * processing engines to allow lower latency for the user. 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and inv_dmp_start() 702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must <b>NOT</b> have been called. 703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param func A user defined callback function. 705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, or non-zero error code otherwise. 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_fifo_processed_callback(void (*func) (void)) 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_process_cb = func; 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Process data from the dmp read via the fifo. Takes a buffer 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * filled with bytes read from the DMP FIFO. 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Currently expects only the 16 bytes of quaternion data. 725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Calculates the motion parameters from that data and stores the 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * results in an internal data structure. 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in,out] dmpData Pointer to the buffer containing the fifo data. 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or error code. 729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall**/ 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_process_fifo_packet(const unsigned char *dmpData) 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int N, kk; 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *p; 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall p = (unsigned char *)(&fifo_obj.decoded); 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall N = fifo_obj.fifo_packet_size; 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (N > sizeof(fifo_obj.decoded)) 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_ASSERTION_FAILURE; 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); 742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < N; ++kk) { 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall p[fifo_obj.offsets[kk]] = *dmpData++; 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // If multiplies are much greater cost than if checks, you could check 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // to see if fifo_scale is non-zero first, or equal to (1L<<30) 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < REF_LAST; ++kk) { 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[kk] = 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]); 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS], 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long)); 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_PROCESSED_DATA_READY] = 1; 758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.cache = 0; 759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Converts 16-bit temperature data as read from temperature register 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* into Celcius scaled by 2^16. 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 766895401859313187f15a800e62d43e6bcbf48fadaJP Abgralllong inv_decode_temperature(short tempReg) 767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Celcius = 35 + (T + 3048.7)/325.9 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L); 771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050B1 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Celcius = 35 + (T + 927.4)/360.6 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L); 775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU3050 777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Celcius = 35 + (T + 13200)/280 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L); 779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** @internal 783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* Returns the temperature in hardware units. The scaling may change from part to part. 784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_temperature_raw(short *data) 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) { 791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[2]; 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) { 794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (FIFO_DEBUG) 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Fetching the temperature from the registers\n"); 796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.cache |= FIFO_CACHE_TEMPERATURE; 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(inv_get_serial_handle(), 798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2, 799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs); 800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]); 805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *data = (short)fifo_obj.decoded[REF_RAW]; 808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 1-element vector of temperature. It is read from the hardware if it 813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * doesn't exist in the FIFO. 814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 1-element vector of temperature 815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_temperature(long *data) 818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short tr; 820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_temperature_raw(&tr); 825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = inv_decode_temperature(tr); 830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the Decoded Accel Data. 835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param data 836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a buffer to store the quantized data. 837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_unquantized_accel(long *data) 840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii, kk; 842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) 846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { 849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { 850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk]; 851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the Quantized Accel data algorithm output from the FIFO. 859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param data 860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a buffer to store the quantized data. 861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_quantized_accel(long *data) 864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) 870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { 873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii]; 874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO 880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* and it is read from the registers if it was not put into the FIFO. The data is 881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* cached till the next FIFO processing block time. 882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @param[out] data Length 3, Gyro data 883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_gyro_sensor(long *data) 885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 886895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 887895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 888895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) { 889895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[6]; 891895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) { 892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.cache |= FIFO_CACHE_GYRO; 893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 894895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_read(inv_get_serial_handle(), 895895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6, 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs); 897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 901895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 1] = 902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (((long)regs[0]) << 24) | (((long)regs[1]) << 16); 903895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 2] = 904895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (((long)regs[2]) << 24) | (((long)regs[3]) << 16); 905895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 3] = 906895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (((long)regs[4]) << 24) | (((long)regs[5]) << 16); 907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 908895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Temperature starts at location 0, Gyro at location 1. 909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 1] = 910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult(fifo_obj.decoded[REF_RAW + 1], 911895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_scale[REF_RAW + 1]); 912895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 2] = 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult(fifo_obj.decoded[REF_RAW + 2], 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_scale[REF_RAW + 2]); 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_RAW + 3] = 916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult(fifo_obj.decoded[REF_RAW + 3], 917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_scale[REF_RAW + 3]); 918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = fifo_obj.decoded[REF_RAW + 1]; 920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = fifo_obj.decoded[REF_RAW + 2]; 921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = fifo_obj.decoded[REF_RAW + 3]; 922895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long data2[6]; 924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_gyro_and_accel_sensor(data2); 925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = data2[0]; 926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = data2[1]; 927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = data2[2]; 928895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 6-element vector of gyro and accel data 934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 6-element vector of gyro and accel data 935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_gyro_and_accel_sensor(long *data) 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_RAW_DATA]) 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) { 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii]; 948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 952895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of external sensor 955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 3-element vector of external sensor 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_external_sensor_data(long *data, int size) 959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL]) 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 969895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < size && ii < 6; ii++) { 970895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii]; 971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 972895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(data, 0, COMPASS_NUM_AXES * sizeof(long)); 976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 979895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Sends accelerometer data to the FIFO. 982895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a subset. 986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. Set to zero to remove it from the FIFO. 989895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy) 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 }; 994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 997895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL, 1001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall KEY_CFG_12, CONFIG_ACCEL); 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens; 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1011895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1012895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits. 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all 1018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * together for a subset. 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. Set to zero to remove it from the FIFO. 1023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy) 1025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t kk; 1028895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1029895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; 1030895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4); 1035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA); 1036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy & INV_16_BIT) { 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAF8 + 2; 1039895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1040895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1041895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy; 1042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) == 0) 1045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk + 1] = DINAA0 + 3; 1046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 1047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_1, 5, regs); 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1053895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1054895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1056895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1057895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1058895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Adds a rolling counter to the fifo packet. When used with the footer 1060895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the data comes out the first time: 1061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @code 1063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <data0><data1>...<dataN><PacketNum0><PacketNum1> 1064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endcode 1065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for every other packet it is 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @code 1068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1> 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endcode 1070895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This allows for scanning of the fifo for packets 1072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1073895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or error code 1074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_packet_number(uint_fast16_t accuracy) 1076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs; 1080895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t elements; 1081895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1084895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1); 1086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 1) { 1087895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs = DINA28; 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_ELEMENT_1 | INV_16_BIT; 1090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs = DINAF8 + 3; 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0; 1093895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s); 1095895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1096895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1097895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1098895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1099895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Send the computed gravity vectors into the FIFO. 1105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The gravity vectors can be retrieved from the FIFO via 1106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_get_gravity(), to have the gravitation vector expressed 1107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * in coordinates relative to the body. 1108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Gravity is a derived vector derived from the quaternion. 1110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param elements 1111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the gravitation vectors components bitmask. 1112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * To send all compoents use INV_ALL. 1113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param accuracy 1114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The number of bits the gravitation vector is expressed 1115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * into. 1116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. 1118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Set to zero to remove it from the FIFO. 1119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy) 1123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_quaternion(accuracy); 1128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sends gyro data to the FIFO. Gyro data is a 3 length vector 1137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). 1138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them 1139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a subset. 1141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. Set to zero to remove it from the FIFO. 1143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy) 1145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 }; 1148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) { 1154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINA90 + 5; 1155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); 1156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAF8 + 1; 1161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA20; 1162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA28; 1163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINA30; 1164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINA90 + 10; 1166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); 1167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAF8 + 1; 1172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA28; 1173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA30; 1174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINA38; 1175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS, 1177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall KEY_CFG_9, CONFIG_GYROS); 1178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sends linear accelerometer data to the FIFO. 1183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Linear accelerometer data is a 3 length vector of 32-bits. It is the 1185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * acceleration in the body frame with gravity removed. 1186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of 1189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a subset. 1191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES 1193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. Set to zero to remove it from the FIFO. 1195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_linear_accel(uint_fast16_t elements, 1197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t accuracy) 1198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char state = inv_get_state(); 1202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (state < INV_STATE_DMP_OPENED) 1204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_gravity(elements, accuracy); 1207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_accel(elements, accuracy); 1212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sends linear world accelerometer data to the FIFO. Linear world 1221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * accelerometer data is a 3 length vector of 32-bits. It is the acceleration 1222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * in the world frame with gravity removed. Should be called once after 1223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_dmp_open() and before inv_dmp_start(). 1224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of 1226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a subset. 1228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. 1230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, 1232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t accuracy) 1233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_linear_accel(INV_ALL, accuracy); 1238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_quaternion(accuracy); 1243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector 1248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). 1249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 1250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bit data. 1251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_quaternion(uint_fast16_t accuracy) 1253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, 1256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINA30, DINA38 1257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t elements, kk; 1259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4); 1265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT); 1266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy & INV_16_BIT) { 1268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAF8 + 2; 1269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy; 1272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) == 0) 1275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk + 1] = DINAA0 + 3; 1276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 1277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_8, 5, regs); 1280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sends raw data to the FIFO. 1289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Should be called once after inv_dmp_open() and before inv_dmp_start(). 1290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them 1291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together 1292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a subset. The first element is temperature, the next 3 are gyro data, 1293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and the last 3 accel data. 1294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param accuracy 1295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off. 1296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 if successful, a non-zero error code otherwise. 1297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy) 1299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int result; 1301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 1302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 1303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, 1304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, 1305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3 1306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy) 1312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_16_BIT; 1313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); 1315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 1) 1317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; 1318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; 1320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0x7e) 1321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_RAW_DATA] = 1322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (0x3f & (elements >> 1)) | INV_16_BIT; 1323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_RAW_DATA] = 0; 1325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_1) { 1327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINACA; 1328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_2) { 1330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINBC4; 1331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_3) { 1333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINACC; 1334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_4) { 1336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINBC6; 1337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_5) { 1339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[4] = DINBC0; 1340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_6) { 1342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[5] = DINAC8; 1343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_7) { 1345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[6] = DINBC2; 1346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_15, 7, regs); 1348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4] = { DINAA0 + 3, 1358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3, 1359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3, 1360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3 1361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy) 1367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_16_BIT; 1368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); 1370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0x03) { 1372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements |= 0x03; 1373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINA20; 1374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0x0C) { 1376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements |= 0x0C; 1377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA28; 1378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0x30) { 1380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements |= 0x30; 1381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA30; 1382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0x40) { 1384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements |= 0xC0; 1385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINA38; 1386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_15, 4, regs); 1389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0x01) 1395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; 1396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; 1398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & 0xfe) 1399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_RAW_DATA] = 1400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (0x7f & (elements >> 1)) | INV_16_BIT; 1401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_RAW_DATA] = 0; 1403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Sends raw external data to the FIFO. 1409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Should be called once after inv_dmp_open() and before inv_dmp_start(). 1410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them 1411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together 1412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for a subset. 1413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data. 1414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Sending and Stop sending are reference counted, so data actually 1415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * stops when the reference reaches zero. 1416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_external_sensor_data(uint_fast16_t elements, 1418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t accuracy) 1419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 1421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 1422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int result; 1423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3, 1424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3, DINAA0 + 3, 1425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINAA0 + 3, DINAA0 + 3 }; 1426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy) 1431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_16_BIT; 1432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6); 1434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements) 1436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT; 1437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0; 1439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_1) { 1441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINBC2; 1442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_2) { 1444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINACA; 1445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_3) { 1447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINBC4; 1448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_4) { 1450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINBC0; 1451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_5) { 1453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[4] = DINAC8; 1454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements & INV_ELEMENT_6) { 1456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[5] = DINACC; 1457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs); 1460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported 1469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Send the Quantized Acceleromter data into the FIFO. The data can be 1474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel(). 1475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * To be useful this should be set to fifo_rate + 1 if less than 1477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work. 1478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param elements 1480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the components bitmask. 1481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * To send all compoents use INV_ALL. 1482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param accuracy 1484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Use INV_32_BIT for 32-bit data or INV_16_BIT for 1485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 16-bit data. 1486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Set to zero to remove it from the FIFO. 1487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 1489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_quantized_accel(uint_fast16_t elements, 1491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast16_t accuracy) 1492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, 1495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINA30, DINA38 1496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs2[4] = { DINA20, DINA28, 1498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall DINA30, DINA38 1499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t kk; 1502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t ii; 1503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8); 1508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (elements) { 1510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT; 1511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0; 1513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) { 1516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0; 1517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { 1518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded_accel[kk][ii] = 0; 1519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) == 0) 1524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk + 1] = DINAA0 + 3; 1525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 1526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs); 1529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) == 0) 1536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs2[kk] = DINAA0 + 3; 1537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 1538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2); 1541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy) 1550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t kk; 1553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[3] = { DINA28, DINA30, DINA38 }; 1554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 1557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accuracy) { 1560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = INV_32_BIT; 1561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3); 1564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS); 1565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.data_config[CONFIG_EIS] = elements | accuracy; 1567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 1569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((elements & 1) == 0) 1570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk] = DINAA0 + 7; 1571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall elements >>= 1; 1572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs); 1575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_footer(); 1577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of accelerometer data in body frame. 1581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 3-element vector of accelerometer data in body frame. 1583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * One gee = 2^16. 1584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 1585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_accel(long *data) 1587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((!fifo_obj.data_config[CONFIG_ACCEL] && 1595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) 1596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || 1597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && 1598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall !inv_accel_present())) 1599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_ACCEL + kk]; 1603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 4-element quaternion vector derived from 6-axis or 1610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is 1611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * gyros, accel and compass. 1612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 4-element quaternion vector. One is scaled to 2^30. 1614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 1615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_quaternion(long *data) 1617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_QUAT]) 1624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_QUATERNION + kk]; 1628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 4-element quaternion vector derived from 6 1635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * axis sensors (gyros and accels). 1636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 1637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 4-element quaternion vector. One is scaled to 2^30. 1638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 1639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_6axis_quaternion(long *data) 1641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_QUAT]) 1647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk]; 1651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_relative_quaternion(long *data) 1657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 1660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = inv_obj.relative_quat[0]; 1661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = inv_obj.relative_quat[1]; 1662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = inv_obj.relative_quat[2]; 1663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[3] = inv_obj.relative_quat[3]; 1664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of gyro data in body frame. 1669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 1670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 3-element vector of gyro data in body frame 1671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with gravity removed. One degree per second = 2^16. 1672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 1673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_gyro(long *data) 1675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.data_config[CONFIG_GYROS]) { 1681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 1682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_GYROS + kk]; 1683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the 3-element gravity vector from the FIFO expressed 1692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * in coordinates relative to the body frame. 1693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param data 1694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 3-element vector of gravity in body frame. 1695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 1696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_gravity(long *data) 1698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long quat[4]; 1700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 1701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_QUAT]) 1707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) { 1710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY; 1711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Compute it from Quaternion 1713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_quaternion(quat); 1714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = 1720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); 1721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = 1722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); 1723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = 1724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) - 1725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1073741824L; 1726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { 1728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii] >>= 14; 1729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.gravity_cache[ii] = data[ii]; 1730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = fifo_obj.gravity_cache[0]; 1733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = fifo_obj.gravity_cache[1]; 1734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = fifo_obj.gravity_cache[2]; 1735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @brief Sets the filter coefficent used for computing the acceleration 1742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* bias which is used to compute linear acceleration. 1743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @param[in] coef Fitler coefficient. 0. means no filter, a small number means 1744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* a small cutoff frequency with an increasing number meaning 1745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* an increasing cutoff frequency. 1746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 1747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_linear_accel_filter_coef(float coef) 1748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.acc_filter_coef = coef; 1750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of accelerometer data in body frame 1755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with gravity removed. 1756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 3-element vector of accelerometer data in body frame 1757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with gravity removed. One g = 2^16. 1758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. data unchanged on error. 1759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_linear_accel(long *data) 1761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long grav[3]; 1764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long la[3]; 1765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_gravity(grav); 1771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_accel(la); 1776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) { 1782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.cache |= FIFO_CACHE_ACC_BIAS; 1783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long x; 1786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall x = la[kk] - grav[kk]; 1787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef + 1788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.acc_bias_filt[kk] * 1789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (1.f - 1790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.acc_filter_coef)); 1791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = x - fifo_obj.acc_bias_filt[kk]; 1792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk]; 1796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of accelerometer data in world frame 1803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with gravity removed. 1804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 3-element vector of accelerometer data in world frame 1805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with gravity removed. One g = 2^16. 1806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success or an error code. 1807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_linear_accel_in_world(long *data) 1809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) { 1814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long wtemp[4], qi[4], wtemp2[4]; 1815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall wtemp[0] = 0; 1816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_linear_accel(&wtemp[1]); 1817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2); 1818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi); 1819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q_mult(wtemp2, qi, wtemp); 1820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 1821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = wtemp[kk + 1]; 1822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 4-element vector of control data. 1831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 4-element vector of control data. 1832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 for succes or an error code. 1833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_cntrl_data(long *data) 1835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_CONTROL_DATA]) 1841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_CONTROL + kk]; 1845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of EIS shfit data 1853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 3-element vector of EIS shift data. 1854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 for succes or an error code. 1855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_eis(long *data) 1857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_EIS]) 1863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 1866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_EIS + kk]; 1867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 3-element vector of accelerometer data in body frame. 1875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 3-element vector of accelerometer data in body frame in g's. 1876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 for success or an error code. 1877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_accel_float(float *data) 1879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long lData[3]; 1881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int result; 1883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1886895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1887895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_accel(lData); 1888895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1889895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1891895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 1894895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = lData[kk] / 65536.0f; 1895895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1901895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns 4-element quaternion vector. 1902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] data 4-element quaternion vector. 1903895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 0 on success, an error code otherwise. 1904895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1905895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_quaternion_float(float *data) 1906895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1908895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (data == NULL) 1910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 1911895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1912895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!fifo_obj.data_config[CONFIG_QUAT]) 1913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_ENABLED; 1914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 1916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f; 1917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1922895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Command the MPU to put data in the FIFO at a particular rate. 1924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For 1926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * example if the MPU is running at 200Hz the following values apply: 1927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1928895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TABLE> 1929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR> 1930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR> 1931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR> 1932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR> 1933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR> 1934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR> 1935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR> 1936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * </TABLE> 1937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED) 1939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * then inv_run_state_callbacks() will be called to allow features 1940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * that depend upon fundamental constants to be updated. 1941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and inv_dmp_start() 1948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must <b>NOT</b> have been called. 1949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param fifoRate Divider value - 1. Output rate is 1951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (DMP Sample Rate) / (fifoRate + 1). 1952895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, ML error code on any failure. 1954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_fifo_rate(unsigned short fifoRate) 1956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[2]; 1959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char state; 1960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 1961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall state = inv_get_state(); 1964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED) 1965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_rate = fifoRate; 1968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1969895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { 1970895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = (unsigned char)((fifoRate >> 8) & 0xff); 1972895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = (unsigned char)(fifoRate & 0xff); 1973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_0_22, 2, regs); 1975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1979895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.sample_step_size_ms = 1980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned short)(((long)fifoRate + 1) * 1981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (inv_mpu_get_sampling_period_us 1982895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldl_cfg)) / 1000L); 1983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (state == INV_STATE_DMP_STARTED) 1985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_run_state_callbacks(state); 1986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) { 1987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 1988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long data; 1989895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_CONFIG_ODR_RESUME; 1990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = sizeof(long); 1991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = (state == INV_STATE_DMP_STARTED); 1992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = &data; 1993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1); 1994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Ask for the same frequency */ 1996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_config_accel(mldl_cfg, 1997895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 1998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 1999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 2000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 2001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_get_accel_config(mldl_cfg, 2004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 2005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 2006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 2007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 2008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(FIFO_DEBUG) 2011895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000); 2012895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Record the actual frequency granted odr is in mHz */ 2013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data); 2014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Retrieve the current FIFO update divider - 1. 2020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * See inv_set_fifo_rate() for how this value is used. 2021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The fifo rate when there is no fifo is the equivilent divider when 2023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * derived from the value set by SetSampleSteSizeMs() 2024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error. 2026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned short inv_get_fifo_rate(void) 2028895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2029895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return fifo_obj.fifo_rate; 2030895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns the step size for quaternion type data. 2034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Typically the data rate for each FIFO packet. When the gryos are sleeping 2036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * this value will return the last value set by SetSampleStepSizeMs() 2037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return step size for quaternion type data 2039895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2040895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint_fast16_t inv_get_sample_step_size_ms(void) 2041895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 2043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) 2045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (fifo_obj.fifo_rate + 1) * 2046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000); 2047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 2048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return fifo_obj.sample_step_size_ms; 2049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Returns the step size for quaternion type data. 2053895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2054895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Typically the data rate for each FIFO packet. When the gryos are sleeping 2055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * this value will return the last value set by SetSampleStepSizeMs() 2056895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2057895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return step size for quaternion type data 2058895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint_fast16_t inv_get_sample_frequency(void) 2060895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 2062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) 2064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (inv_mpu_get_sampling_rate_hz(mldl_cfg) / 2065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (fifo_obj.fifo_rate + 1)); 2066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 2067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (1000 / fifo_obj.sample_step_size_ms); 2068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2070895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The gyro data magnitude squared : 2072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. 2073895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return the computed magnitude squared output of the gyroscope. 2074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned long inv_get_gyro_sum_of_sqr(void) 2076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long gmag = 0; 2078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long temp; 2079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 2080895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2081895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 2082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = fifo_obj.decoded[REF_GYROS + kk] >> 2083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (16 - (GYRO_MAG_SQR_SHIFT / 2)); 2084895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gmag += temp * temp; 2085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2087895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return gmag; 2088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The gyro data magnitude squared: 2092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT. 2093895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return the computed magnitude squared output of the accelerometer. 2094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2095895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned long inv_accel_sum_of_sqr(void) 2096895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2097895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long amag = 0; 2098895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long temp; 2099895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 2100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accel[3]; 2101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 2102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_accel(accel); 2104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 2105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 2106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 2109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2)); 2110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall amag += temp * temp; 2111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return amag; 2113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 2117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_override_quaternion(float *q) 2119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 2121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 4; ++kk) { 2122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30)); 2123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 2128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief This registers a function to be called for each set of 2129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * gyro/quaternion/rotation matrix/etc output. 2130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] func The callback function to register 2131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] priority The unique priority number of the callback. Lower numbers 2132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * are called first. 2133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return error code. 2134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority) 2136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 2138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 2139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk, nn; 2140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_lock_mutex(fifo_rate_obj.mutex); 2142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 2143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure we haven't registered this function already 2146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Or used the same priority 2147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { 2148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((fifo_rate_obj.fifo_process_cb[kk] == func) || 2149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (fifo_rate_obj.priority[kk] == priority)) { 2150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_unlock_mutex(fifo_rate_obj.mutex); 2151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 2152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure we have not filled up our number of allowable callbacks 2156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) { 2157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall kk = 0; 2158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_rate_obj.num_cb != 0) { 2159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // set kk to be where this new callback goes in the array 2160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall while ((kk < fifo_rate_obj.num_cb) && 2161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (fifo_rate_obj.priority[kk] < priority)) { 2162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall kk++; 2163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (kk != fifo_rate_obj.num_cb) { 2165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // We need to move the others 2166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) { 2167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.fifo_process_cb[nn] = 2168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.fifo_process_cb[nn - 1]; 2169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1]; 2170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Add new callback 2174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.fifo_process_cb[kk] = func; 2175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.priority[kk] = priority; 2176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.num_cb++; 2177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 2178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = INV_ERROR_MEMORY_EXAUSTED; 2179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_unlock_mutex(fifo_rate_obj.mutex); 2182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 2186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 2187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief This unregisters a function to be called for each set of 2188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * gyro/quaternion/rotation matrix/etc output. 2189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return error code. 2190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 2191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_unregister_fifo_rate_process(inv_obj_func func) 2192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 2194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk, jj; 2195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 2196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_lock_mutex(fifo_rate_obj.mutex); 2198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 2199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure we haven't registered this function already 2202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = INV_ERROR_INVALID_PARAMETER; 2203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { 2204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_rate_obj.fifo_process_cb[kk] == func) { 2205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) { 2206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.fifo_process_cb[jj - 1] = 2207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.fifo_process_cb[jj]; 2208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.priority[jj - 1] = 2209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.priority[jj]; 2210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL; 2212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0; 2213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_rate_obj.num_cb--; 2214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = INV_SUCCESS; 2215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 2216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_unlock_mutex(fifo_rate_obj.mutex); 2220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef UMPL 2224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallbool bFIFIDataAvailable = FALSE; 2225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallbool isUmplDataInFIFO(void) 2226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return bFIFIDataAvailable; 2228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid setUmplDataInFIFOFlag(bool flag) 2230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bFIFIDataAvailable = flag; 2232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 2234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_run_fifo_rate_processes(void) 2235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 2236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 2237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result, result2; 2238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_lock_mutex(fifo_rate_obj.mutex); 2240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 2241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("MLOsLockMutex returned %d\n", result); 2242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // User callbacks take priority over the fifo_process_cb callback 2245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_obj.fifo_process_cb) 2246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifo_obj.fifo_process_cb(); 2247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { 2249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (fifo_rate_obj.fifo_process_cb[kk]) { 2250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj); 2251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == INV_SUCCESS) 2252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef UMPL 2253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setUmplDataInFIFOFlag(TRUE); 2254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 2255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = result2; 2256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 2258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_unlock_mutex(fifo_rate_obj.mutex); 2260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 2261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 2262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 2263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/*********************/ 2264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /** \}*//* defgroup */ 2265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/*********************/ 2266