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 *
21 * $Id: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
22 *
23 *****************************************************************************/
24
25 /**
26 * @addtogroup MLDL
27 *
28 * @{
29 * @file mldl_cfg_mpu.c
30 * @brief The Motion Library Driver Layer.
31 */
32
33 /* ------------------ */
34 /* - Include Files. - */
35 /* ------------------ */
36
37 #include <stddef.h>
38 #include "mldl_cfg.h"
39 #include "mlsl.h"
40 #include "mpu.h"
41
42 #ifdef LINUX
43 #include <sys/ioctl.h>
44 #endif
45
46 #include "log.h"
47 #undef MPL_LOG_TAG
48 #define MPL_LOG_TAG "MPL-mldl_cfg_mpu:"
49
50 /* --------------------- */
51 /* - Variables. - */
52 /* --------------------- */
53
54
55 /* ---------------------- */
56 /* - Static Functions. - */
57 /* ---------------------- */
mpu_print_cfg(struct mldl_cfg * mldl_cfg)58 void mpu_print_cfg(struct mldl_cfg * mldl_cfg)
59 {
60 struct mpu_platform_data *pdata = mldl_cfg->pdata;
61 struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel;
62 struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass;
63 struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure;
64
65 MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr);
66 MPL_LOGD("mldl_cfg.int_config = %02x\n", mldl_cfg->int_config);
67 MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync);
68 MPL_LOGD("mldl_cfg.full_scale = %02x\n", mldl_cfg->full_scale);
69 MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf);
70 MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src);
71 MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider);
72 MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", mldl_cfg->dmp_enable);
73 MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", mldl_cfg->fifo_enable);
74 MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1);
75 MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2);
76 MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", mldl_cfg->offset_tc[0]);
77 MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", mldl_cfg->offset_tc[1]);
78 MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", mldl_cfg->offset_tc[2]);
79 MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision);
80 MPL_LOGD("mldl_cfg.product_id = %02x\n", mldl_cfg->product_id);
81 MPL_LOGD("mldl_cfg.gyro_sens_trim = %02x\n", mldl_cfg->gyro_sens_trim);
82 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
83 MPL_LOGD("mldl_cfg.accel_sens_trim = %02x\n", mldl_cfg->accel_sens_trim);
84 #endif
85
86 if (mldl_cfg->accel) {
87 MPL_LOGD("slave_accel->suspend = %02x\n", (int)mldl_cfg->accel->suspend);
88 MPL_LOGD("slave_accel->resume = %02x\n", (int)mldl_cfg->accel->resume);
89 MPL_LOGD("slave_accel->read = %02x\n", (int)mldl_cfg->accel->read);
90 MPL_LOGD("slave_accel->type = %02x\n", mldl_cfg->accel->type);
91 MPL_LOGD("slave_accel->read_reg = %02x\n",
92 mldl_cfg->accel->read_reg);
93 MPL_LOGD("slave_accel->read_len = %02x\n",
94 mldl_cfg->accel->read_len);
95 MPL_LOGD("slave_accel->endian = %02x\n", mldl_cfg->accel->endian);
96 MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa);
97 MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction);
98 } else {
99 MPL_LOGD("slave_accel = NULL\n");
100 }
101
102 if (mldl_cfg->compass) {
103 MPL_LOGD("slave_compass->suspend = %02x\n", (int)mldl_cfg->compass->suspend);
104 MPL_LOGD("slave_compass->resume = %02x\n", (int)mldl_cfg->compass->resume);
105 MPL_LOGD("slave_compass->read = %02x\n", (int)mldl_cfg->compass->read);
106 MPL_LOGD("slave_compass->type = %02x\n", mldl_cfg->compass->type);
107 MPL_LOGD("slave_compass->read_reg = %02x\n",
108 mldl_cfg->compass->read_reg);
109 MPL_LOGD("slave_compass->read_len = %02x\n",
110 mldl_cfg->compass->read_len);
111 MPL_LOGD("slave_compass->endian = %02x\n", mldl_cfg->compass->endian);
112 MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa);
113 MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction);
114 } else {
115 MPL_LOGD("slave_compass = NULL\n");
116 }
117
118 if (mldl_cfg->pressure) {
119 MPL_LOGD("slave_pressure->suspend = %02x\n", (int)mldl_cfg->pressure->suspend);
120 MPL_LOGD("slave_pressure->resume = %02x\n", (int)mldl_cfg->pressure->resume);
121 MPL_LOGD("slave_pressure->read = %02x\n", (int)mldl_cfg->pressure->read);
122 MPL_LOGD("slave_pressure->type = %02x\n", mldl_cfg->pressure->type);
123 MPL_LOGD("slave_pressure->read_reg = %02x\n",
124 mldl_cfg->pressure->read_reg);
125 MPL_LOGD("slave_pressure->read_len = %02x\n",
126 mldl_cfg->pressure->read_len);
127 MPL_LOGD("slave_pressure->endian = %02x\n", mldl_cfg->pressure->endian);
128 MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa);
129 MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction);
130 } else {
131 MPL_LOGD("slave_pressure = NULL\n");
132 }
133
134 MPL_LOGD("accel->get_slave_descr = %x\n",(unsigned int) accel->get_slave_descr);
135 MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num);
136 MPL_LOGD("accel->bus = %02x\n", accel->bus);
137 MPL_LOGD("accel->address = %02x\n", accel->address);
138 MPL_LOGD("accel->orientation = \n"
139 " %2d %2d %2d\n"
140 " %2d %2d %2d\n"
141 " %2d %2d %2d\n",
142 accel->orientation[0],accel->orientation[1],accel->orientation[2],
143 accel->orientation[3],accel->orientation[4],accel->orientation[5],
144 accel->orientation[6],accel->orientation[7],accel->orientation[8]);
145 MPL_LOGD("compass->get_slave_descr = %x\n",(unsigned int) compass->get_slave_descr);
146 MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num);
147 MPL_LOGD("compass->bus = %02x\n", compass->bus);
148 MPL_LOGD("compass->address = %02x\n", compass->address);
149 MPL_LOGD("compass->orientation = \n"
150 " %2d %2d %2d\n"
151 " %2d %2d %2d\n"
152 " %2d %2d %2d\n",
153 compass->orientation[0],compass->orientation[1],compass->orientation[2],
154 compass->orientation[3],compass->orientation[4],compass->orientation[5],
155 compass->orientation[6],compass->orientation[7],compass->orientation[8]);
156 MPL_LOGD("pressure->get_slave_descr = %x\n",(unsigned int) pressure->get_slave_descr);
157 MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num);
158 MPL_LOGD("pressure->bus = %02x\n", pressure->bus);
159 MPL_LOGD("pressure->address = %02x\n", pressure->address);
160 MPL_LOGD("pressure->orientation = \n"
161 " %2d %2d %2d\n"
162 " %2d %2d %2d\n"
163 " %2d %2d %2d\n",
164 pressure->orientation[0],pressure->orientation[1],pressure->orientation[2],
165 pressure->orientation[3],pressure->orientation[4],pressure->orientation[5],
166 pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]);
167
168 MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config);
169 MPL_LOGD("pdata->level_shifter = %02x\n", pdata->level_shifter);
170 MPL_LOGD("pdata->orientation = \n"
171 " %2d %2d %2d\n"
172 " %2d %2d %2d\n"
173 " %2d %2d %2d\n",
174 pdata->orientation[0],pdata->orientation[1],pdata->orientation[2],
175 pdata->orientation[3],pdata->orientation[4],pdata->orientation[5],
176 pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]);
177
178 MPL_LOGD("Struct sizes: mldl_cfg: %d, "
179 "ext_slave_descr:%d, mpu_platform_data:%d: RamOffset: %d\n",
180 sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr),
181 sizeof(struct mpu_platform_data),
182 offsetof(struct mldl_cfg, ram));
183 }
184
185 /******************************************************************************
186 ******************************************************************************
187 * Exported functions
188 ******************************************************************************
189 *****************************************************************************/
190
191 /**
192 * Initializes the pdata structure to defaults.
193 *
194 * Opens the device to read silicon revision, product id and whoami. Leaves
195 * the device in suspended state for low power.
196 *
197 * @param mldl_cfg handle to the config structure
198 * @param mlsl_handle handle to the mpu serial layer
199 * @param accel_handle handle to the accel serial layer
200 * @param compass_handle handle to the compass serial layer
201 * @param pressure_handle handle to the pressure serial layer
202 *
203 * @return INV_SUCCESS if silicon revision, product id and woami are supported
204 * by this software.
205 */
inv_mpu_open(struct mldl_cfg * mldl_cfg,void * mlsl_handle,void * accel_handle,void * compass_handle,void * pressure_handle)206 int inv_mpu_open(struct mldl_cfg *mldl_cfg,
207 void *mlsl_handle,
208 void *accel_handle,
209 void *compass_handle,
210 void *pressure_handle)
211 {
212 int result;
213 result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
214 if (result) {
215 LOG_RESULT_LOCATION(result);
216 return result;
217 }
218
219 result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
220 INV_ALL_SENSORS);
221 if (result) {
222 LOG_RESULT_LOCATION(result);
223 return result;
224 }
225 return result;
226 }
227
228 /**
229 * Stub for driver close. Just verify that the devices are suspended
230 *
231 * @param mldl_cfg handle to the config structure
232 * @param mlsl_handle handle to the mpu serial layer
233 * @param accel_handle handle to the accel serial layer
234 * @param compass_handle handle to the compass serial layer
235 * @param pressure_handle handle to the compass serial layer
236 *
237 * @return INV_SUCCESS or non-zero error code
238 */
inv_mpu_close(struct mldl_cfg * mldl_cfg,void * mlsl_handle,void * accel_handle,void * compass_handle,void * pressure_handle)239 int inv_mpu_close(struct mldl_cfg *mldl_cfg,
240 void *mlsl_handle,
241 void *accel_handle,
242 void *compass_handle,
243 void *pressure_handle)
244 {
245 int result = INV_SUCCESS;
246
247 result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
248 INV_ALL_SENSORS);
249 return result;
250 }
251
inv_mpu_resume(struct mldl_cfg * mldl_cfg,void * mlsl_handle,void * accel_handle,void * compass_handle,void * pressure_handle,unsigned long sensors)252 int inv_mpu_resume(struct mldl_cfg* mldl_cfg,
253 void *mlsl_handle,
254 void *accel_handle,
255 void *compass_handle,
256 void *pressure_handle,
257 unsigned long sensors)
258 {
259 int result;
260
261 mldl_cfg->requested_sensors = sensors;
262 result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
263 if (result) {
264 LOG_RESULT_LOCATION(result);
265 return result;
266 }
267 result = ioctl((int)mlsl_handle, MPU_RESUME, NULL);
268 if (result) {
269 LOG_RESULT_LOCATION(result);
270 return result;
271 }
272 result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
273 if (result) {
274 LOG_RESULT_LOCATION(result);
275 return result;
276 }
277 //MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors);
278
279 return result;
280 }
281
282
inv_mpu_suspend(struct mldl_cfg * mldl_cfg,void * mlsl_handle,void * accel_handle,void * compass_handle,void * pressure_handle,unsigned long sensors)283 int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
284 void *mlsl_handle,
285 void *accel_handle,
286 void *compass_handle,
287 void *pressure_handle,
288 unsigned long sensors)
289 {
290 int result;
291 unsigned long requested = mldl_cfg->requested_sensors;
292
293 mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
294 //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__,
295 // mldl_cfg->requested_sensors);
296
297 result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
298 if (result) {
299 LOG_RESULT_LOCATION(result);
300 return result;
301 }
302 result = ioctl((int)mlsl_handle, MPU_SUSPEND, NULL);
303 if (result) {
304 LOG_RESULT_LOCATION(result);
305 return result;
306 }
307 result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
308 if (result) {
309 LOG_RESULT_LOCATION(result);
310 return result;
311 }
312
313 mldl_cfg->requested_sensors = requested;
314 //MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested);
315
316 return result;
317 }
318
319 /**
320 * Send slave configuration information
321 *
322 * @param mldl_cfg pointer to the mldl configuration structure
323 * @param gyro_handle handle to the gyro sensor
324 * @param slave_handle handle to the slave sensor
325 * @param slave slave description
326 * @param pdata slave platform data
327 * @param data where to store the read data
328 *
329 * @return 0 or non-zero error code
330 */
inv_mpu_slave_read(struct mldl_cfg * mldl_cfg,void * gyro_handle,void * slave_handle,struct ext_slave_descr * slave,struct ext_slave_platform_data * pdata,unsigned char * data)331 int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
332 void *gyro_handle,
333 void *slave_handle,
334 struct ext_slave_descr *slave,
335 struct ext_slave_platform_data *pdata,
336 unsigned char *data)
337 {
338 int result;
339 if (!mldl_cfg || !gyro_handle || !data || !slave) {
340 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
341 return INV_ERROR_INVALID_PARAMETER;
342 }
343
344 switch (slave->type) {
345 case EXT_SLAVE_TYPE_ACCELEROMETER:
346 result = ioctl((int)gyro_handle, MPU_READ_ACCEL, data);
347 break;
348 case EXT_SLAVE_TYPE_COMPASS:
349 result = ioctl((int)gyro_handle, MPU_READ_COMPASS, data);
350 break;
351 case EXT_SLAVE_TYPE_PRESSURE:
352 result = ioctl((int)gyro_handle, MPU_READ_PRESSURE, data);
353 break;
354 default:
355 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
356 return INV_ERROR_INVALID_PARAMETER;
357 break;
358 }
359
360 return result;
361 }
362
363 /**
364 * Send slave configuration information
365 *
366 * @param mldl_cfg pointer to the mldl configuration structure
367 * @param gyro_handle handle to the gyro sensor
368 * @param slave_handle handle to the slave sensor
369 * @param data the data being sent
370 * @param slave slave description
371 * @param pdata slave platform data
372 *
373 * @return 0 or non-zero error code
374 */
inv_mpu_slave_config(struct mldl_cfg * mldl_cfg,void * gyro_handle,void * slave_handle,struct ext_slave_config * data,struct ext_slave_descr * slave,struct ext_slave_platform_data * pdata)375 int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
376 void *gyro_handle,
377 void *slave_handle,
378 struct ext_slave_config *data,
379 struct ext_slave_descr *slave,
380 struct ext_slave_platform_data *pdata)
381 {
382 int result;
383 if (!mldl_cfg || !data || !slave || !pdata || !slave) {
384 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
385 return INV_ERROR_INVALID_PARAMETER;
386 }
387
388 switch (slave->type) {
389 case EXT_SLAVE_TYPE_ACCELEROMETER:
390 result = ioctl((int)gyro_handle, MPU_CONFIG_ACCEL, data);
391 if (result) {
392 LOG_RESULT_LOCATION(result);
393 return result;
394 }
395 break;
396 case EXT_SLAVE_TYPE_COMPASS:
397 result = ioctl((int)gyro_handle, MPU_CONFIG_COMPASS, data);
398 if (result) {
399 LOG_RESULT_LOCATION(result);
400 return result;
401 }
402 break;
403 case EXT_SLAVE_TYPE_PRESSURE:
404 result = ioctl((int)gyro_handle, MPU_CONFIG_PRESSURE, data);
405 if (result) {
406 LOG_RESULT_LOCATION(result);
407 return result;
408 }
409 break;
410 default:
411 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
412 return INV_ERROR_INVALID_PARAMETER;
413 break;
414 }
415
416 return result;
417 }
418
419 /**
420 * Request slave configuration information
421 *
422 * Use this specifically after requesting a slave configuration to see what the
423 * slave accually accepted.
424 *
425 * @param mldl_cfg pointer to the mldl configuration structure
426 * @param gyro_handle handle to the gyro sensor
427 * @param slave_handle handle to the slave sensor
428 * @param data the data being requested.
429 * @param slave slave description
430 * @param pdata slave platform data
431 *
432 * @return 0 or non-zero error code
433 */
inv_mpu_get_slave_config(struct mldl_cfg * mldl_cfg,void * gyro_handle,void * slave_handle,struct ext_slave_config * data,struct ext_slave_descr * slave,struct ext_slave_platform_data * pdata)434 int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
435 void *gyro_handle,
436 void *slave_handle,
437 struct ext_slave_config *data,
438 struct ext_slave_descr *slave,
439 struct ext_slave_platform_data *pdata)
440 {
441 int result;
442 if (!mldl_cfg || !data || !slave) {
443 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
444 return INV_ERROR_INVALID_PARAMETER;
445 }
446 switch (slave->type) {
447 case EXT_SLAVE_TYPE_ACCELEROMETER:
448 result = ioctl((int)gyro_handle, MPU_GET_CONFIG_ACCEL, data);
449 if (result) {
450 LOG_RESULT_LOCATION(result);
451 return result;
452 }
453 break;
454 case EXT_SLAVE_TYPE_COMPASS:
455 result = ioctl((int)gyro_handle, MPU_GET_CONFIG_COMPASS, data);
456 if (result) {
457 LOG_RESULT_LOCATION(result);
458 return result;
459 }
460 break;
461 case EXT_SLAVE_TYPE_PRESSURE:
462 result = ioctl((int)gyro_handle, MPU_GET_CONFIG_PRESSURE, data);
463 if (result) {
464 LOG_RESULT_LOCATION(result);
465 return result;
466 }
467 break;
468 default:
469 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
470 return INV_ERROR_INVALID_PARAMETER;
471 break;
472 }
473 return result;
474 }
475 /**
476 *@}
477 */
478