1 /* 2 $License: 3 Copyright 2011 InvenSense, Inc. 4 5 Licensed under the Apache License, Version 2.0 (the "License"); 6 you may not use this file except in compliance with the License. 7 You may obtain a copy of the License at 8 9 http://www.apache.org/licenses/LICENSE-2.0 10 11 Unless required by applicable law or agreed to in writing, software 12 distributed under the License is distributed on an "AS IS" BASIS, 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 See the License for the specific language governing permissions and 15 limitations under the License. 16 $ 17 */ 18 /******************************************************************************* 19 * 20 * $Id: accel.c 4595 2011-01-25 01:43:03Z mcaramello $ 21 * 22 *******************************************************************************/ 23 24 /** 25 * @defgroup ACCELDL 26 * @brief Motion Library - Accel Driver Layer. 27 * Provides the interface to setup and handle an accel 28 * connected to either the primary or the seconday I2C interface 29 * of the gyroscope. 30 * 31 * @{ 32 * @file accel.c 33 * @brief Accel setup and handling methods. 34 **/ 35 36 /* ------------------ */ 37 /* - Include Files. - */ 38 /* ------------------ */ 39 40 #include <string.h> 41 42 #include "ml.h" 43 #include "mlinclude.h" 44 #include "dmpKey.h" 45 #include "mlFIFO.h" 46 #include "mldl.h" 47 #include "mldl_cfg.h" 48 #include "mlMathFunc.h" 49 #include "mlsl.h" 50 #include "mlos.h" 51 52 #include "log.h" 53 #undef MPL_LOG_TAG 54 #define MPL_LOG_TAG "MPL-accel" 55 56 #define ACCEL_DEBUG 0 57 58 /* --------------------- */ 59 /* - Global Variables. - */ 60 /* --------------------- */ 61 62 /* --------------------- */ 63 /* - Static Variables. - */ 64 /* --------------------- */ 65 66 /* --------------- */ 67 /* - Prototypes. - */ 68 /* --------------- */ 69 70 /* -------------- */ 71 /* - Externs. - */ 72 /* -------------- */ 73 74 /* -------------- */ 75 /* - Functions. - */ 76 /* -------------- */ 77 78 /** 79 * @brief Used to determine if an accel is configured and 80 * used by the MPL. 81 * @return INV_SUCCESS if the accel is present. 82 */ 83 unsigned char inv_accel_present(void) 84 { 85 INVENSENSE_FUNC_START; 86 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 87 if (NULL != mldl_cfg->accel && 88 NULL != mldl_cfg->accel->resume && 89 mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) 90 return TRUE; 91 else 92 return FALSE; 93 } 94 95 /** 96 * @brief Query the accel slave address. 97 * @return The 7-bit accel slave address. 98 */ 99 unsigned char inv_get_slave_addr(void) 100 { 101 INVENSENSE_FUNC_START; 102 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 103 if (NULL != mldl_cfg->pdata) 104 return mldl_cfg->pdata->accel.address; 105 else 106 return 0; 107 } 108 109 /** 110 * @brief Get the ID of the accel in use. 111 * @return ID of the accel in use. 112 */ 113 unsigned short inv_get_accel_id(void) 114 { 115 INVENSENSE_FUNC_START; 116 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 117 if (NULL != mldl_cfg->accel) { 118 return mldl_cfg->accel->id; 119 } 120 return ID_INVALID; 121 } 122 123 /** 124 * @brief Get a sample of accel data from the device. 125 * @param data 126 * the buffer to store the accel raw data for 127 * X, Y, and Z axes. 128 * @return INV_SUCCESS or a non-zero error code. 129 */ 130 inv_error_t inv_get_accel_data(long *data) 131 { 132 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 133 inv_error_t result; 134 unsigned char raw_data[2 * ACCEL_NUM_AXES]; 135 long tmp[ACCEL_NUM_AXES]; 136 int ii; 137 signed char *mtx = mldl_cfg->pdata->accel.orientation; 138 char accelId = mldl_cfg->accel->id; 139 140 if (NULL == data) 141 return INV_ERROR_INVALID_PARAMETER; 142 143 if (mldl_cfg->accel->read_len > sizeof(raw_data)) 144 return INV_ERROR_ASSERTION_FAILURE; 145 146 result = (inv_error_t) inv_mpu_read_accel(mldl_cfg, 147 inv_get_serial_handle(), 148 inv_get_serial_handle(), 149 raw_data); 150 if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { 151 return result; 152 } 153 if (result) { 154 LOG_RESULT_LOCATION(result); 155 return result; 156 } 157 158 for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { 159 if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) { 160 tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256; 161 tmp[ii] += (long)((unsigned char)raw_data[2 * ii]); 162 } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) || 163 (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) { 164 tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256; 165 tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]); 166 if (accelId == ACCEL_ID_KXSD9) { 167 tmp[ii] = (long)((short)(((unsigned short)tmp[ii]) 168 + ((unsigned short)0x8000))); 169 } 170 } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) { 171 tmp[ii] = (long)((signed char)raw_data[ii]) * 256; 172 } else { 173 result = INV_ERROR_FEATURE_NOT_IMPLEMENTED; 174 } 175 } 176 177 for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { 178 data[ii] = ((long)tmp[0] * mtx[3 * ii] + 179 (long)tmp[1] * mtx[3 * ii + 1] + 180 (long)tmp[2] * mtx[3 * ii + 2]); 181 } 182 183 //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]); 184 return result; 185 } 186 187 /** 188 * @} 189 */ 190
