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: ml.c 5642 2011-06-14 02:26:20Z mcaramello $ 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *****************************************************************************/ 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup ML 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Motion Library APIs. 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The Motion Library processes gyroscopes, accelerometers, and 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * compasses to provide a physical model of the movement for the 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sensors. 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The results of this processing may be used to control objects 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * within a user interface environment, detect gestures, track 3D 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * movement for gaming applications, and analyze the blur created 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * due to hand movement while taking a picture. 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file ml.c 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief The Motion Library APIs. 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ------------------ */ 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Include Files. - */ 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ------------------ */ 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mltypes.h" 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlinclude.h" 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpKey.h" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpDefault.h" 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlstates.h" 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFOHW.h" 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc.h" 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsupervisor.h" 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlmath.h" 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlcontrol.h" 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mpu.h" 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "accel.h" 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlBiasNoMotion.h" 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-ml" 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_MOT_TYPE_NONE 0 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_MOT_TYPE_NO_MOTION 1 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_MOT_TYPE_MOTION_DETECTED 2 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_MOT_STATE_MOVING 0 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_MOT_STATE_NO_MOTION 1 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ML_MOT_STATE_BIAS_IN_PROG 2 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define _mlDebug(x) //{x} 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* Global Variables */ 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallconst unsigned char mlVer[] = { INV_VERSION }; 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct inv_params_obj inv_params_obj = { 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_ORIENTATION_MASK_DEFAULT, // orientation_mask 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_STATE_SERIAL_CLOSED // starting state 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall}; 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct inv_obj_t inv_obj; 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid *g_mlsl_handle; 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgralltypedef struct { 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // These describe callbacks happening everythime a DMP interrupt is processed 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t numInterruptProcesses; 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Array of function pointers, function being void function taking void 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES]; 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} tMLXCallbackInterrupt; // MLX_callback_t 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 104895401859313187f15a800e62d43e6bcbf48fadaJP AbgralltMLXCallbackInterrupt mlxCallbackInterrupt; 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------- */ 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Functions. - */ 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------- */ 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient); 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient); 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Open serial connection with the MPU device. 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This is the entry point of the MPL and must be 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * called prior to any other function call. 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param port System handle for 'port' MPU device is found on. 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The significance of this parameter varies by 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * platform. It is passed as 'port' to MLSLSerialOpen. 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or error code. 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_serial_start(char const *port) 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() >= INV_STATE_SERIAL_OPENED) 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_state_transition(INV_STATE_SERIAL_OPENED); 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_open(port, &g_mlsl_handle); 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (void)inv_state_transition(INV_STATE_SERIAL_CLOSED); 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Close the serial communication. 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This function needs to be called explicitly to shut down 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the communication with the device. Calling inv_dmp_close() 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * won't affect the established serial communication. 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS; non-zero error code otherwise. 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_serial_stop(void) 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() == INV_STATE_SERIAL_CLOSED) 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_state_transition(INV_STATE_SERIAL_CLOSED); 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result); 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_close(g_mlsl_handle); 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result); 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the serial file handle to the device. 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return The serial file handle. 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid *inv_get_serial_handle(void) 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return g_mlsl_handle; 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief apply the choosen orientation and full scale range 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * for gyroscopes, accelerometer, and compass. 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero code otherwise. 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_apply_calibration(void) 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall signed char gyroCal[9] = { 0 }; 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall signed char accelCal[9] = { 0 }; 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall signed char magCal[9] = { 0 }; 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float gyroScale = 2000.f; 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float accelScale = 0.f; 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float magScale = 0.f; 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < 9; ii++) { 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroCal[ii] = mldl_cfg->pdata->orientation[ii]; 204a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (NULL != mldl_cfg->accel){ 205a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii]; 206a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow } 207a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (NULL != mldl_cfg->compass){ 208a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow magCal[ii] = mldl_cfg->pdata->compass.orientation[ii]; 209a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow } 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (mldl_cfg->full_scale) { 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case MPU_FS_250DPS: 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroScale = 250.f; 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case MPU_FS_500DPS: 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroScale = 500.f; 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case MPU_FS_1000DPS: 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroScale = 1000.f; 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case MPU_FS_2000DPS: 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroScale = 2000.f; 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n", 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldl_cfg->full_scale); 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 232a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (NULL != mldl_cfg->accel){ 233a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale); 234a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow inv_obj.accel_sens = (long)(accelScale * 65536L); 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */ 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim; 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_sens /= 2; 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 242a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow } 243a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (NULL != mldl_cfg->compass){ 244a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale); 245a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow inv_obj.compass_sens = (long)(magScale * 32768); 246a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow } 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() == INV_STATE_DMP_OPENED) { 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_calibration(gyroScale, gyroCal); 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (INV_SUCCESS != result) { 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGE("Unable to set Gyro Calibration\n"); 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 254a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (NULL != mldl_cfg->accel){ 255a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow result = inv_set_accel_calibration(accelScale, accelCal); 256a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (INV_SUCCESS != result) { 257a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow MPL_LOGE("Unable to set Accel Calibration\n"); 258a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow return result; 259a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow } 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 261a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (NULL != mldl_cfg->compass){ 262a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow result = inv_set_compass_calibration(magScale, magCal); 263a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow if (INV_SUCCESS != result) { 264a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow MPL_LOGE("Unable to set Mag Calibration\n"); 265a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow return result; 266a45503dc4dc8d9331bbc9e3fce1b1c35cb57c39fRosa Chow } 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Setup the DMP to handle the accelerometer endianess. 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, a non-zero error code otherwise. 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_apply_endian_accel(void) 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4] = { 0 }; 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int endian = mldl_cfg->accel->endian; 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) { 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall endian = EXT_SLAVE_BIG_ENDIAN; 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (endian) { 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_FS8_BIG_ENDIAN: 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_FS16_BIG_ENDIAN: 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_LITTLE_ENDIAN: 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = 0; 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = 64; 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = 0; 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = 0; 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case EXT_SLAVE_BIG_ENDIAN: 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = 0; 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = 0; 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = 64; 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = 0; 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_set_mpu_memory(KEY_D_1_236, 4, regs); 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Initialize MLX data. This should be called to setup the mlx 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * output buffers before any motion processing is done. 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_init_ml(void) 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long tmp[COMPASS_NUM_AXES]; 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Set all values to zero by default 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&inv_obj, 0, sizeof(struct inv_obj_t)); 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt)); 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_correction[0] = 1073741824L; 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_correction_relative[0] = 1073741824L; 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_disturb_correction[0] = 1073741824L; 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_correction_offset[0] = 1073741824L; 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.relative_quat[0] = 1073741824L; 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Not used with the ST accelerometer 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.no_motion_threshold = 20; // noMotionThreshold 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Not used with the ST accelerometer 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.motion_duration = 1536; // motionDuration 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.motion_state = INV_MOTION; // Motion state 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.bias_update_time = 8000; 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.bias_calc_time = 2000; 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.internal_motion_state = ML_MOT_STATE_MOVING; 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.start_time = inv_get_tick_count(); 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_cal[0] = 322122560L; 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_cal[4] = 322122560L; 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_cal[8] = 322122560L; 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed. 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < COMPASS_NUM_AXES; ii++) { 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_scale[ii] = 65536L; 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_test_scale[ii] = 65536L; 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[ii] = P_INIT; 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[ii] = 0; 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_asa[ii] = (1L << 30); 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_compass_read_scale(tmp) == INV_SUCCESS) { 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < COMPASS_NUM_AXES; ii++) 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_asa[ii] = tmp[ii]; 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_no_motion_bias = 0; 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_compass_bias = 0; 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_state = SF_UNCALIBRATED; 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_STARTUP_SETTLE; 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_init_compass_bias = 0; 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.resetting_compass = 0; 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.external_slave_callback = NULL; 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_accuracy = 0; 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.factory_temp_comp = 0; 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_coarse_heading = 0; 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[0] = P_INIT; 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[4] = P_INIT; 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_ptr[8] = P_INIT; 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.gyro_bias_err = 1310720; 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_lpf_gain = 1073744L; 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.no_motion_accel_threshold = 7000000L; 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief This registers a function to be called for each time the DMP 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * generates an an interrupt. 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * It will be called after inv_register_fifo_rate_process() which gets called 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * every time the FIFO data is processed. 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The FIFO does not have to be on for this callback. 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param func Function to be called when a DMP interrupt occurs. 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code. 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_register_dmp_interupt_cb(inv_obj_func func) 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure we have not filled up our number of allowable callbacks 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mlxCallbackInterrupt.numInterruptProcesses <= 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MAX_INTERRUPT_PROCESSES - 1) { 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure we haven't registered this function already 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Add new callback 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt. 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall numInterruptProcesses] = func; 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlxCallbackInterrupt.numInterruptProcesses++; 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_MEMORY_EXAUSTED; 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief This unregisters a function to be called for each DMP interrupt. 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code. 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func) 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk, jj; 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Make sure we haven't registered this function already 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // FIXME, we may need a thread block here 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses; 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ++jj) { 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlxCallbackInterrupt.processInterruptCb[jj - 1] = 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlxCallbackInterrupt.processInterruptCb[jj]; 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlxCallbackInterrupt.numInterruptProcesses--; 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_PARAMETER; 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Run the recorded interrupt process callbacks in the event 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of an interrupt. 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_run_dmp_interupt_cb(void) 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mlxCallbackInterrupt.processInterruptCb[kk]) 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj); 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** @internal 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* Resets the Motion/No Motion state which should be called at startup and resume. 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_reset_motion(void) 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[8]; 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.motion_state = INV_MOTION; 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.no_motion_accel_time = inv_get_tick_count(); 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU3050 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAD8 + 2; 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA0C; 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINAD8 + 1; 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(regs, 0, 8); 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_96, 8, regs); 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs)); 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_motion_state(INV_MOTION); 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_start_bias_calc starts the bias calculation on the MPU. 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_start_bias_calc(void) 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.suspend = 1; 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_stop_bias_calc stops the bias calculation on the MPU. 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_stop_bias_calc(void) 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.suspend = 0; 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_update_data fetches data from the fifo and updates the 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * motion algorithms. 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and inv_dmp_start() must have been called. 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @note Motion algorithm data is constant between calls to inv_update_data 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_SUCCESS 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - Non-zero error code 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_update_data(void) 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t got, ftry; 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t mpu_interrupt; 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() != INV_STATE_DMP_STARTED) 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Set the maximum number of FIFO packets we want to process 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ftry = 100; // Large enough to process all packets 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ftry = 1; 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Go and process at most ftry number of packets, probably less than ftry 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_read_and_process_fifo(ftry, &got); 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Process all interrupts 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1); 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mpu_interrupt) { 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_clear_interrupt_trigger(INTSRC_AUX1); 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Check if interrupt was from MPU 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU); 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mpu_interrupt) { 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_clear_interrupt_trigger(INTSRC_MPU); 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Take care of the callbacks that want to be notified when there was an MPU interrupt 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mpu_interrupt) { 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_run_dmp_interupt_cb(); 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_fifo_status(); 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_check_flag returns the value of a flag. 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_check_flag can be used to check a number of flags, 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * allowing users to poll flags rather than register callback 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * functions. If a flag is set to True when inv_check_flag is called, 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the flag is automatically reset. 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The flags are: 588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_RAW_DATA_READY 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Indicates that new raw data is available. 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_PROCESSED_DATA_READY 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Indicates that new processed data is available. 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_GOT_GESTURE 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Indicates that a gesture has been detected by the gesture engine. 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_MOTION_STATE_CHANGE 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Indicates that a change has been made from motion to no motion, 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or vice versa. 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and inv_dmp_start() must have been called. 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param flag The flag to check. 606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return TRUE or FALSE state of the flag 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_check_flag(int flag) 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int flagReturn = inv_obj.flags[flag]; 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[flag] = 0; 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return flagReturn; 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Enable generation of the DMP interrupt when Motion or no-motion 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is detected 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param on 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Boolean to turn the interrupt on or off. 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code. 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_motion_interrupt(unsigned char on) 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[2] = { 0 }; 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (on) { 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.interrupt_sources |= INV_INT_MOTION; 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.interrupt_sources &= ~INV_INT_MOTION; 643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!inv_obj.interrupt_sources) { 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_dl_cfg_int(0); 645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (on) { 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAFE; 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAD8; 656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_7, 1, regs); 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Enable generation of the DMP interrupt when a FIFO packet is ready 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param on Boolean to turn the interrupt on or off 669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_fifo_interrupt(unsigned char on) 673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[2] = { 0 }; 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (on) { 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.interrupt_sources |= INV_INT_FIFO; 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.interrupt_sources &= ~INV_INT_FIFO; 690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!inv_obj.interrupt_sources) { 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_dl_cfg_int(0); 692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (on) { 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAFE; 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINAD8; 703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_CFG_6, 1, regs); 705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the current set of DMP interrupt sources. 714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * These interrupts are generated by the DMP and can be 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * routed to the MPU interrupt line via internal 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * settings. 717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Currently enabled interrupt sources. The possible interrupts are: 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_INT_FIFO, 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_INT_MOTION, 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_INT_TAP 729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_get_interrupts(void) 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() < INV_STATE_DMP_OPENED) 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; // error 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_obj.interrupt_sources; 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Sets up the Accelerometer calibration and scale factor. 742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Please refer to the provided "9-Axis Sensor Fusion Application 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Note" document provided. Section 5, "Sensor Mounting Orientation" 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * offers a good coverage on the mounting matrices and explains how 746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * to use them. 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_start() must <b>NOT</b> have been called. 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @see inv_set_gyro_calibration(). 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @see inv_set_compass_calibration(). 758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] range 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The range of the accelerometers in g's. An accelerometer 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * that has a range of +2g's to -2g's should pass in 2. 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] orientation 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A 9 element matrix that represents how the accelerometers 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * are oriented with respect to the device they are mounted 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * in and the reference axis system. 766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. 767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This example corresponds to a 3 x 3 identity matrix. 768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful; a non-zero error code otherwise. 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_accel_calibration(float range, signed char *orientation) 772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float cal[9]; 775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float scale = range / 32768.f; 776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long sf; 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4] = { 0, 0, 0, 0 }; 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() != INV_STATE_DMP_OPENED) 783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Apply zero g-offset values */ 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) { 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = 0x80; 788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = 0x0; 789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = 0x80; 790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = 0x0; 791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmpkey_supported(KEY_D_1_152)) { 794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]); 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (scale == 0) { 802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_sens = 0; 803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->accel->id) { 806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C }; 809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmp[3] = { DINA4C, DINACD, DINA6C }; 811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[3]; 814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short orient; 815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 9; ++kk) { 817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall cal[kk] = scale * orientation[kk]; 818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); 819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall orient = inv_orientation_matrix_to_scalar(orientation); 822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = tmp[orient & 3]; 823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = tmp[(orient >> 3) & 3]; 824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = tmp[(orient >> 6) & 3]; 825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs); 826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINA26; 832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA46; 833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2 834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA66; 835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision) 837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall == MPU_PRODUCT_KEY_B1_E1_5) 838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA76; 839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA66; 841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (orient & 4) 843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] |= 1; 844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (orient & 0x20) 845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] |= 1; 846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (orient & 0x100) 847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] |= 1; 848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs); 850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) { 856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_freescale_sensor_fusion_16bit(orient); 857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) { 862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_freescale_sensor_fusion_8bit(orient); 863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.accel_sens != 0) { 871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = (1073741824L / inv_obj.accel_sens); 872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = 0; 874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = (unsigned char)((sf >> 8) & 0xff); 876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = (unsigned char)(sf & 0xff); 877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_0_108, 2, regs); 878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 886895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 887895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Sets up the Gyro calibration and scale factor. 888895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 889895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Please refer to the provided "9-Axis Sensor Fusion Application 890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Note" document provided. Section 5, "Sensor Mounting Orientation" 891895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * offers a good coverage on the mounting matrices and explains 892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * how to use them. 893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 894895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 895895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_start() must have <b>NOT</b> been called. 901895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @see inv_set_accel_calibration(). 903895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @see inv_set_compass_calibration(). 904895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 905895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] range 906895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The range of the gyros in degrees per second. A gyro 907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * that has a range of +2000 dps to -2000 dps should pass in 908895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 2000. 909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] orientation 910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A 9 element matrix that represents how the gyro are oriented 911895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with respect to the device they are mounted in. A typical 912895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * example corresponds to a 3 x 3 identity matrix. 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_gyro_calibration(float range, signed char *orientation) 918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 922895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float scale; 924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[12] = { 0 }; 927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char maxVal = 0; 928895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpPtr = 0; 929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpSign = 0; 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char i = 0; 931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int sf = 0; 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() != INV_STATE_DMP_OPENED) 934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->gyro_sens_trim != 0) { 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* adjust the declared range to consider the gyro_sens_trim 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall of this part when different from the default (first dividend) */ 939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim; 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall scale = range / 32768.f; // inverse of sensitivity for the given full scale range 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.gyro_sens = (long)(range * 32768L); 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 9; ++kk) { 946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30)); 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30)); 948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 952895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpD = DINA4C; 954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpE = DINACD; 955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpF = DINA6C; 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpD = DINAC9; 958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpE = DINA2C; 959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char tmpF = DINACB; 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINA36; 962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[4] = DINA56; 963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[5] = DINA76; 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall maxVal = 0; 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmpSign = 0; 968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.gyro_orient[0 + 3 * i] < 0) 969895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmpSign = 1; 970895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ABS(inv_obj.gyro_orient[1 + 3 * i]) > 971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ABS(inv_obj.gyro_orient[0 + 3 * i])) { 972895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall maxVal = 1; 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.gyro_orient[1 + 3 * i] < 0) 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmpSign = 1; 975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ABS(inv_obj.gyro_orient[2 + 3 * i]) > 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ABS(inv_obj.gyro_orient[1 + 3 * i])) { 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmpSign = 0; 979895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall maxVal = 2; 980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.gyro_orient[2 + 3 * i] < 0) 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmpSign = 1; 982895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (maxVal == 0) { 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[tmpPtr++] = tmpD; 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmpSign) 986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[tmpPtr + 2] |= 0x01; 987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (maxVal == 1) { 988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[tmpPtr++] = tmpE; 989895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmpSign) 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[tmpPtr + 2] |= 0x01; 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[tmpPtr++] = tmpF; 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmpSign) 994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[tmpPtr + 2] |= 0x01; 995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 997895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs); 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]); 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.gyro_sf = 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL); 1011895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 1012895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory(KEY_D_0_104, 4, 1013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_int32_to_big8(inv_obj.gyro_sf, regs)); 1014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.gyro_sens != 0) { 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens); 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = 0; 1023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs)); 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1028895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1029895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1030895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Sets up the Compass calibration and scale factor. 1036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Please refer to the provided "9-Axis Sensor Fusion Application 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Note" document provided. Section 5, "Sensor Mounting Orientation" 1039895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * offers a good coverage on the mounting matrices and explains 1040895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * how to use them. 1041895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_start() must have <b>NOT</b> been called. 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @see inv_set_gyro_calibration(). 1051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @see inv_set_accel_calibration(). 1052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1053895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] range 1054895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The range of the compass. 1055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] orientation 1056895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A 9 element matrix that represents how the compass is 1057895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * oriented with respect to the device they are mounted in. 1058895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. 1059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This example corresponds to a 3 x 3 identity matrix. 1060895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The matrix describes how to go from the chip mounting to 1061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the body of the device. 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_compass_calibration(float range, signed char *orientation) 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float cal[9]; 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float scale = range / 32768.f; 1070895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short compassId = 0; 1072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1073895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassId = inv_get_compass_id(); 1074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883) 1076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (compassId == COMPASS_ID_LSM303M)) { 1077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall scale /= 32.0f; 1078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1080895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 9; ++kk) { 1081895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall cal[kk] = scale * orientation[kk]; 1082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); 1083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1084895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sens = (long)(scale * 1073741824L); 1086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1087895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) { 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char reg0[4] = { 0, 0, 0, 0 }; 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regp[4] = { 64, 0, 0, 0 }; 1090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regn[4] = { 64 + 128, 0, 0, 0 }; 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *reg; 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int_fast8_t kk; 1093895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short keyList[9] = 1094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02, 1095895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12, 1096895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22 1097895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 1098895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1099895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 9; ++kk) { 1100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (orientation[kk] == 1) 1102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = regp; 1103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (orientation[kk] == -1) 1104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = regn; 1105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = reg0; 1107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory(keyList[kk], 4, reg); 1108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @internal 1116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup. 1117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 1118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_dead_zone(void) 1119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char reg; 1121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall extern struct control_params cntrl_params; 1123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (cntrl_params.functions & INV_DEAD_ZONE) { 1125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = 0x08; 1126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 1128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 1129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = 0; 1130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 1131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { 1132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = 0x2; 1133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall reg = 0; 1135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_0_163, 1, ®); 1140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_set_bias_update is used to register which algorithms will be 1149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * used to automatically reset the gyroscope bias. 1150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The engine INV_BIAS_UPDATE must be enabled for these algorithms to 1151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * run. 1152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and inv_dmp_start() 1159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must <b>NOT</b> have been called. 1160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param function A function or bitwise OR of functions that determine 1162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * how the gyroscope bias will be automatically updated. 1163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Functions include: 1164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_NONE or 0, 1165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_BIAS_FROM_NO_MOTION, 1166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_BIAS_FROM_GRAVITY, 1167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_BIAS_FROM_TEMPERATURE, 1168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall \ifnot UMPL 1169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_BIAS_FROM_LPF, 1170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_MAG_BIAS_FROM_MOTION, 1171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_MAG_BIAS_FROM_GYRO, 1172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * - INV_ALL. 1173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * \endif 1174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_bias_update(unsigned short function) 1177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4]; 1180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long tmp[3] = { 0, 0, 0 }; 1181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 1182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_state() != INV_STATE_DMP_OPENED) 1185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* do not allow progressive no motion bias tracker to run - 1188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall it's not fully debugged */ 1189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround 1190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n"); 1191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // You must use EnableFastNoMotion() to get this feature 1193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall function &= ~INV_BIAS_FROM_FAST_NO_MOTION; 1194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // You must use DisableFastNoMotion() to turn this feature off 1196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) 1197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall function |= INV_BIAS_FROM_FAST_NO_MOTION; 1198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*--- remove magnetic components from bias tracking 1200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if there is no compass ---*/ 1201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!inv_compass_present()) { 1202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION); 1203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall function &= ~(INV_BIAS_FROM_LPF); 1205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Enable function sets bias from no motion 1208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION); 1209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (function & INV_BIAS_FROM_NO_MOTION) { 1211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_enable_bias_no_motion(); 1212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_disable_bias_no_motion(); 1214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { 1217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINA80 + 2; 1218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA2D; 1219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA55; 1220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINA7D; 1221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = DINA80 + 7; 1223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = DINA2D; 1224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = DINA35; 1225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = DINA3D; 1226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs); 1228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_dead_zone(); 1233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) && 1239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall !inv_compass_present()) { 1240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION); 1241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW); 1247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.factory_temp_comp = 0; // FIXME, workaround 1254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((mldl_cfg->offset_tc[0] != 0) || 1255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) { 1256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.factory_temp_comp = 1; 1257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.factory_temp_comp == 0) { 1260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) { 1261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_temp_slope(inv_obj.temp_slope); 1262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_temp_slope(tmp); 1268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE; 1275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("factory temperature compensation coefficients available - " 1276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n"); 1277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on 1280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compass and accel data, is to have accelerometer data delivered in the 1281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FIFO ----*/ 1282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) 1283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall && inv_compass_present()) 1284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) 1285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) { 1286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_send_accel(INV_ALL, INV_32_BIT); 1287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_send_gyro(INV_ALL, INV_32_BIT); 1288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_get_motion_state is used to determine if the device is in 1295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a 'motion' or 'no motion' state. 1296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_get_motion_state returns INV_MOTION of the device is moving, 1297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or INV_NO_MOTION if the device is not moving. 1298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * and inv_dmp_start() 1305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_get_motion_state(void) 1310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_obj.motion_state; 1313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_set_no_motion_thresh is used to set the threshold for 1317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * detecting INV_NO_MOTION 1318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param thresh A threshold scaled in degrees per second. 1327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_no_motion_thresh(float thresh) 1331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 1333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4] = { 0 }; 1334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long tmp; 1335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long)(thresh * thresh * 2.045f); 1338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) { 1339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 1340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (tmp > 8180000L) { 1341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 1342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.no_motion_threshold = tmp; 1344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = (unsigned char)(tmp >> 24); 1346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = (unsigned char)((tmp >> 16) & 0xff); 1347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = (unsigned char)((tmp >> 8) & 0xff); 1348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = (unsigned char)(tmp & 0xff); 1349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_108, 4, regs); 1351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_reset_motion(); 1356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_set_no_motion_threshAccel is used to set the threshold for 1361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * detecting INV_NO_MOTION with accelerometers when Gyros have 1362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * been turned off 1363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param thresh A threshold in g's scaled by 2^32 1372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_no_motion_threshAccel(long thresh) 1376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.no_motion_accel_threshold = thresh; 1380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_set_no_motion_time is used to set the time required for 1386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * detecting INV_NO_MOTION 1387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_dmp_open() 1389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @ifnot MPL_MF 1390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_open_low_power_pedometer() 1391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * or inv_eis_open_dmp() 1392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @endif 1393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * must have been called. 1394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param time A time in seconds. 1396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_no_motion_time(float time) 1400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 1402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[2] = { 0 }; 1403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long tmp; 1404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long)(time * 200); 1408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < 0) { 1409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 1410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (tmp > 65535L) { 1411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 1412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.motion_duration = (unsigned short)tmp; 1414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); 1416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); 1417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); 1418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_reset_motion(); 1423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief inv_get_version is used to get the ML version. 1428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @pre inv_get_version can be called at any time. 1430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param version inv_get_version writes the ML version 1432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * string pointer to version. 1433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful or Non-zero error code otherwise. 1435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_version(unsigned char **version) 1437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *version = (unsigned char *)mlVer; //fixme we are wiping const 1441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 1443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Check for the presence of the gyro sensor. 1447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * This is not a physical check but a logical check and the value can change 1449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * dynamically based on calls to inv_set_mpu_sensors(). 1450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return TRUE if the gyro is enabled FALSE otherwise. 1452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint inv_get_gyro_present(void) 1454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO | 1456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_Z_GYRO); 1457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned short inv_row_2_scale(const signed char *row) 1460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short b; 1462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (row[0] > 0) 1464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 0; 1465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (row[0] < 0) 1466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 4; 1467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (row[1] > 0) 1468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 1; 1469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (row[1] < 0) 1470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 5; 1471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (row[2] > 0) 1472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 2; 1473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (row[2] < 0) 1474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 6; 1475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 1476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = 7; // error 1477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return b; 1478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) 1481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short scalar; 1483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 1484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall XYZ 010_001_000 Identity Matrix 1485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall XZY 001_010_000 1486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall YXZ 010_000_001 1487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall YZX 000_010_001 1488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ZXY 001_000_010 1489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ZYX 000_001_010 1490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall scalar = inv_row_2_scale(mtx); 1493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall scalar |= inv_row_2_scale(mtx + 3) << 3; 1494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall scalar |= inv_row_2_scale(mtx + 6) << 6; 1495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return scalar; 1497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* Setups up the Freescale 16-bit accel for Sensor Fusion 1500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @param[in] orient A scalar representation of the orientation 1501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* that describes how to go from the Chip Orientation 1502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* to the Board Orientation often times called the Body Orientation in Invensense Documentation. 1503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. 1504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 1505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient) 1506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char rr[3]; 1508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall orient = orient & 0xdb; 1511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (orient) { 1512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Typically 0x88 1514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[0] = DINACC; 1515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[1] = DINACF; 1516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[2] = DINA0E; 1517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x50: 1519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[0] = DINACE; 1520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[1] = DINA0E; 1521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[2] = DINACD; 1522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x81: 1524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[0] = DINACE; 1525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[1] = DINACB; 1526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[2] = DINA0E; 1527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x11: 1529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[0] = DINACC; 1530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[1] = DINA0E; 1531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[2] = DINACB; 1532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x42: 1534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[0] = DINA0A; 1535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[1] = DINACF; 1536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[2] = DINACB; 1537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x0a: 1539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[0] = DINA0A; 1540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[1] = DINACB; 1541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rr[2] = DINACD; 1542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr); 1545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* Setups up the Freescale 8-bit accel for Sensor Fusion 1549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @param[in] orient A scalar representation of the orientation 1550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* that describes how to go from the Chip Orientation 1551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* to the Board Orientation often times called the Body Orientation in Invensense Documentation. 1552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. 1553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 1554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient) 1555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[27]; 1557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall uint_fast8_t kk; 1559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall orient = orient & 0xdb; 1561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall kk = 0; 1562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAC3; 1564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA90 + 14; 1565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAA0 + 9; 1566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA3E; 1567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA5E; 1568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA7E; 1569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAC2; 1571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAA0 + 9; 1572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA90 + 9; 1573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAF8 + 2; 1574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (orient) { 1576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Typically 0x88 1578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACB; 1579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA54; 1581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACD; 1590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x50: 1592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACB; 1593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACF; 1595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA7C; 1597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x81: 1606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA2C; 1607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACD; 1616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACB; 1618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x11: 1620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA2C; 1621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA28; 1628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACB; 1629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACF; 1630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x42: 1632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACF; 1633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACD; 1634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA7C; 1636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA78; 1643895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case 0x0a: 1645895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACD; 1646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA54; 1648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1650895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA50; 1655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINACF; 1657895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA90 + 7; 1661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAF8 + 3; 1662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAA0 + 9; 1663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA0E; 1664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA0E; 1665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINA0E; 1666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[kk++] = DINAF8 + 1; // filler 1668895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs); 1670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1672895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1678895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Controlls each sensor and each axis when the motion processing unit is 1680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * running. When it is not running, simply records the state for later. 1681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * NOTE: In this version only full sensors controll is allowed. Independent 1683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * axis control will return an error. 1684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param sensors Bit field of each axis desired to be turned on or off 1686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1687895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code 1688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_mpu_sensors(unsigned long sensors) 1690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 1692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char state = inv_get_state(); 1693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 1694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 1695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short fifoRate; 1696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (state < INV_STATE_DMP_OPENED) 1698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SM_IMPROPER_STATE; 1699895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) && 1701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((sensors & INV_THREE_AXIS_ACCEL) != 0)) { 1702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 1703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((sensors & INV_THREE_AXIS_ACCEL) != 0) && 1705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldl_cfg->pdata->accel.get_slave_descr == 0)) { 1706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; 1707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) && 1710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((sensors & INV_THREE_AXIS_COMPASS) != 0)) { 1711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 1712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((sensors & INV_THREE_AXIS_COMPASS) != 0) && 1714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldl_cfg->pdata->compass.get_slave_descr == 0)) { 1715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; 1716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) && 1719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) { 1720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_FEATURE_NOT_IMPLEMENTED; 1721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) && 1723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (mldl_cfg->pdata->pressure.get_slave_descr == 0)) { 1724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; 1725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* DMP was off, and is turning on */ 1728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sensors & INV_DMP_PROCESSOR && 1729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { 1730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 1731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long odr; 1732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_CONFIG_ODR_RESUME; 1733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = sizeof(long); 1734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = (state == INV_STATE_DMP_STARTED); 1735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = &odr; 1736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000); 1738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_config_accel(mldl_cfg, 1739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 1740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 1741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; 1747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall odr = MPU_SLAVE_IRQ_TYPE_NONE; 1748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_config_accel(mldl_cfg, 1749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 1750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 1751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_init_fifo_hardare(); 1756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.mode_change_func) { 1759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors); 1760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Get the fifo rate before changing sensors so if we need to match it */ 1767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fifoRate = inv_get_fifo_rate(); 1768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldl_cfg->requested_sensors = sensors; 1769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* inv_dmp_start will turn the sensors on */ 1771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (state == INV_STATE_DMP_STARTED) { 1772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_dl_start(sensors); 1773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_reset_motion(); 1778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_dl_stop(~sensors); 1783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1784895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1789895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_fifo_rate(fifoRate); 1790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) { 1792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 1793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long data; 1794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = sizeof(long); 1796895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; 1797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = (state == INV_STATE_DMP_STARTED); 1798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = &data; 1799895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data = MPU_SLAVE_IRQ_TYPE_DATA_READY; 1800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_config_accel(mldl_cfg, 1801895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 1802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 1803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1806895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1807895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1808895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_set_mode_change(inv_error_t(*mode_change_func) 1813895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned long, unsigned long)) 1814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1815895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.mode_change_func = mode_change_func; 1816895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1817895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1818895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1819895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* Mantis setup 1820895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 1821895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef CONFIG_MPU_SENSORS_MPU6050B1 1822895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_mpu_6050_config() 1823895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1824895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long temp; 1825895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 1826895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char big8[4]; 1827895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char atc[4]; 1828895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long s[3], s2[3]; 1829895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int kk; 1830895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldlCfg = inv_get_dl_config(); 1831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 1833895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 0x0d, 4, atc); 1834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1835895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1836895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1837895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1838895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1839895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = atc[3] & 0x3f; 1840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (temp >= 32) 1841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = temp - 64; 1842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = (temp << 21) | 0x100000; 1843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp += (1L << 29); 1844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = -temp; 1845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8)); 1846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1850895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 1852895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s[kk] = atc[kk] & 0x3f; 1853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (s[kk] > 32) 1854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s[kk] = s[kk] - 64; 1855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s[kk] *= 2516582L; 1856895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (kk = 0; kk < 3; ++kk) { 1859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] + 1860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] + 1861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldlCfg->pdata->orientation[kk * 3 + 2] * s[2]; 1862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8)); 1864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1865895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1866895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1867895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1868895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8)); 1869895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1873895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8)); 1874895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 1875895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 1876895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1877895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1878895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 1880895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1881895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 1882895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1883895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 1884895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @} 1885895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 1886