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 */
inv_accel_present(void)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 */
inv_get_slave_addr(void)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 */
inv_get_accel_id(void)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 */
inv_get_accel_data(long * data)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