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: mldl.c 5653 2011-06-16 21:06:55Z nroyer $
21 *
22 *****************************************************************************/
23
24 /**
25 * @defgroup MLDL
26 * @brief Motion Library - Driver Layer.
27 * The Motion Library Driver Layer provides the intrface to the
28 * system drivers that are used by the Motion Library.
29 *
30 * @{
31 * @file mldl.c
32 * @brief The Motion Library Driver Layer.
33 */
34
35 /* ------------------ */
36 /* - Include Files. - */
37 /* ------------------ */
38
39 #include <string.h>
40
41 #include "mpu.h"
42 #if defined CONFIG_MPU_SENSORS_MPU6050A2
43 # include "mpu6050a2.h"
44 #elif defined CONFIG_MPU_SENSORS_MPU6050B1
45 # include "mpu6050b1.h"
46 #elif defined CONFIG_MPU_SENSORS_MPU3050
47 # include "mpu3050.h"
48 #else
49 #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
50 #endif
51 #include "mldl.h"
52 #include "mldl_cfg.h"
53 #include "compass.h"
54 #include "mlsl.h"
55 #include "mlos.h"
56 #include "mlinclude.h"
57 #include "ml.h"
58 #include "dmpKey.h"
59 #include "mlFIFOHW.h"
60 #include "compass.h"
61 #include "pressure.h"
62
63 #include "log.h"
64 #undef MPL_LOG_TAG
65 #define MPL_LOG_TAG "MPL-mldl"
66
67 #define _mldlDebug(x) //{x}
68
69 /* --------------------- */
70 /* - Variables. - */
71 /* --------------------- */
72
73 #define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */
74
75 /*---- structure containing control variables used by MLDL ----*/
76 static struct mldl_cfg mldlCfg;
77 struct ext_slave_descr gAccel;
78 struct ext_slave_descr gCompass;
79 struct ext_slave_descr gPressure;
80 struct mpu_platform_data gPdata;
81 static void *sMLSLHandle;
82 int_fast8_t intTrigger[NUM_OF_INTSOURCES];
83
84 /*******************************************************************************
85 * Functions for accessing the DMP memory via keys
86 ******************************************************************************/
87
88 unsigned short (*sGetAddress) (unsigned short key) = NULL;
89 static const unsigned char *localDmpMemory = NULL;
90 static unsigned short localDmpMemorySize = 0;
91
92 /**
93 * @internal
94 * @brief Sets the function to use to convert keys to addresses. This
95 * will changed for each DMP code loaded.
96 * @param func
97 * Function used to convert keys to addresses.
98 * @endif
99 */
inv_set_get_address(unsigned short (* func)(unsigned short key))100 void inv_set_get_address(unsigned short (*func) (unsigned short key))
101 {
102 INVENSENSE_FUNC_START;
103 _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func);
104 )
105 sGetAddress = func;
106 }
107
108 /**
109 * @internal
110 * @brief Check if the feature is supported in the currently loaded
111 * DMP code basing on the fact that the key is assigned a
112 * value or not.
113 * @param key the DMP key
114 * @return whether the feature associated with the key is supported
115 * or not.
116 */
inv_dmpkey_supported(unsigned short key)117 uint_fast8_t inv_dmpkey_supported(unsigned short key)
118 {
119 unsigned short memAddr;
120
121 if (sGetAddress == NULL) {
122 MPL_LOGE("%s : sGetAddress is NULL\n", __func__);
123 return FALSE;
124 }
125
126 memAddr = sGetAddress(key);
127 if (memAddr >= 0xffff) {
128 MPL_LOGV("inv_set_mpu_memory unsupported key\n");
129 return FALSE;
130 }
131
132 return TRUE;
133 }
134
135 /**
136 * @internal
137 * @brief used to get the specified number of bytes from the original
138 * MPU memory location specified by the key.
139 * Reads the specified number of bytes from the MPU location
140 * that was used to program the MPU specified by the key. Each
141 * set of code specifies a function that changes keys into
142 * addresses. This function is set with setGetAddress().
143 *
144 * @param key The key to use when looking up the address.
145 * @param length Number of bytes to read.
146 * @param buffer Result for data.
147 *
148 * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
149 * not corresponding to a memory address will result in INV_ERROR.
150 * @endif
151 */
inv_get_mpu_memory_original(unsigned short key,unsigned short length,unsigned char * buffer)152 inv_error_t inv_get_mpu_memory_original(unsigned short key,
153 unsigned short length,
154 unsigned char *buffer)
155 {
156 unsigned short offset;
157
158 if (sGetAddress == NULL) {
159 return INV_ERROR_NOT_OPENED;
160 }
161
162 offset = sGetAddress(key);
163 if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) {
164 return INV_ERROR_INVALID_PARAMETER;
165 }
166
167 memcpy(buffer, &localDmpMemory[offset], length);
168
169 return INV_SUCCESS;
170 }
171
inv_dl_get_address(unsigned short key)172 unsigned short inv_dl_get_address(unsigned short key)
173 {
174 unsigned short offset;
175 if (sGetAddress == NULL) {
176 return INV_ERROR_NOT_OPENED;
177 }
178
179 offset = sGetAddress(key);
180 return offset;
181 }
182
183 /* ---------------------- */
184 /* - Static Functions. - */
185 /* ---------------------- */
186
187 /**
188 * @brief Open the driver layer and resets the internal
189 * gyroscope, accelerometer, and compass data
190 * structures.
191 * @param mlslHandle
192 * the serial handle.
193 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
194 */
inv_dl_open(void * mlslHandle)195 inv_error_t inv_dl_open(void *mlslHandle)
196 {
197 inv_error_t result;
198 memset(&mldlCfg, 0, sizeof(mldlCfg));
199 memset(intTrigger, INT_CLEAR, sizeof(intTrigger));
200
201 sMLSLHandle = mlslHandle;
202
203 mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */
204 mldlCfg.accel = &gAccel;
205 mldlCfg.compass = &gCompass;
206 mldlCfg.pressure = &gPressure;
207 mldlCfg.pdata = &gPdata;
208
209 result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle,
210 sMLSLHandle, sMLSLHandle, sMLSLHandle);
211 return result;
212 }
213
214 /**
215 * @brief Closes/Cleans up the ML Driver Layer.
216 * Put the device in sleep mode.
217 * @return INV_SUCCESS or non-zero error code.
218 */
inv_dl_close(void)219 inv_error_t inv_dl_close(void)
220 {
221 INVENSENSE_FUNC_START;
222 inv_error_t result = INV_SUCCESS;
223
224 result = (inv_error_t) inv_mpu_suspend(&mldlCfg,
225 sMLSLHandle,
226 sMLSLHandle,
227 sMLSLHandle,
228 sMLSLHandle,
229 INV_ALL_SENSORS);
230
231 result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle,
232 sMLSLHandle, sMLSLHandle, sMLSLHandle);
233 /* Clear all previous settings */
234 memset(&mldlCfg, 0, sizeof(mldlCfg));
235 sMLSLHandle = NULL;
236 sGetAddress = NULL;
237 return result;
238 }
239
240 /**
241 * @brief Sets the requested_sensors
242 *
243 * Accessor to set the requested_sensors field of the mldl_cfg structure.
244 * Typically set at initialization.
245 *
246 * @param sensors
247 * Bitfield of the sensors that are going to be used. Combination of the
248 * following:
249 * - INV_X_GYRO
250 * - INV_Y_GYRO
251 * - INV_Z_GYRO
252 * - INV_DMP_PROCESSOR
253 * - INV_X_ACCEL
254 * - INV_Y_ACCEL
255 * - INV_Z_ACCEL
256 * - INV_X_COMPASS
257 * - INV_Y_COMPASS
258 * - INV_Z_COMPASS
259 * - INV_X_PRESSURE
260 * - INV_Y_PRESSURE
261 * - INV_Z_PRESSURE
262 * - INV_THREE_AXIS_GYRO
263 * - INV_THREE_AXIS_ACCEL
264 * - INV_THREE_AXIS_COMPASS
265 * - INV_THREE_AXIS_PRESSURE
266 *
267 * @return INV_SUCCESS or non-zero error code
268 */
inv_init_requested_sensors(unsigned long sensors)269 inv_error_t inv_init_requested_sensors(unsigned long sensors)
270 {
271 mldlCfg.requested_sensors = sensors;
272
273 return INV_SUCCESS;
274 }
275
276 /**
277 * @brief Starts the DMP running
278 *
279 * Resumes the sensor if any of the sensor axis or components are requested
280 *
281 * @param sensors
282 * Bitfield of the sensors to turn on. Combination of the following:
283 * - INV_X_GYRO
284 * - INV_Y_GYRO
285 * - INV_Z_GYRO
286 * - INV_DMP_PROCESSOR
287 * - INV_X_ACCEL
288 * - INV_Y_ACCEL
289 * - INV_Z_ACCEL
290 * - INV_X_COMPASS
291 * - INV_Y_COMPASS
292 * - INV_Z_COMPASS
293 * - INV_X_PRESSURE
294 * - INV_Y_PRESSURE
295 * - INV_Z_PRESSURE
296 * - INV_THREE_AXIS_GYRO
297 * - INV_THREE_AXIS_ACCEL
298 * - INV_THREE_AXIS_COMPASS
299 * - INV_THREE_AXIS_PRESSURE
300 *
301 * @return INV_SUCCESS or non-zero error code
302 */
inv_dl_start(unsigned long sensors)303 inv_error_t inv_dl_start(unsigned long sensors)
304 {
305 INVENSENSE_FUNC_START;
306 inv_error_t result = INV_SUCCESS;
307
308 mldlCfg.requested_sensors = sensors;
309 result = inv_mpu_resume(&mldlCfg,
310 sMLSLHandle,
311 sMLSLHandle,
312 sMLSLHandle,
313 sMLSLHandle,
314 sensors);
315 return result;
316 }
317
318 /**
319 * @brief Stops the DMP running and puts it in low power as requested
320 *
321 * Suspends each sensor according to the bitfield, if all axis and components
322 * of the sensor is off.
323 *
324 * @param sensors Bitfiled of the sensors to leave on. Combination of the
325 * following:
326 * - INV_X_GYRO
327 * - INV_Y_GYRO
328 * - INV_Z_GYRO
329 * - INV_X_ACCEL
330 * - INV_Y_ACCEL
331 * - INV_Z_ACCEL
332 * - INV_X_COMPASS
333 * - INV_Y_COMPASS
334 * - INV_Z_COMPASS
335 * - INV_X_PRESSURE
336 * - INV_Y_PRESSURE
337 * - INV_Z_PRESSURE
338 * - INV_THREE_AXIS_GYRO
339 * - INV_THREE_AXIS_ACCEL
340 * - INV_THREE_AXIS_COMPASS
341 * - INV_THREE_AXIS_PRESSURE
342 *
343 *
344 * @return INV_SUCCESS or non-zero error code
345 */
inv_dl_stop(unsigned long sensors)346 inv_error_t inv_dl_stop(unsigned long sensors)
347 {
348 INVENSENSE_FUNC_START;
349 inv_error_t result = INV_SUCCESS;
350
351 result = inv_mpu_suspend(&mldlCfg,
352 sMLSLHandle,
353 sMLSLHandle,
354 sMLSLHandle,
355 sMLSLHandle,
356 sensors);
357 return result;
358 }
359
360 /**
361 * @brief Get a pointer to the internal data structure
362 * storing the configuration for the MPU, the accelerometer
363 * and the compass in use.
364 * @return a pointer to the data structure of type 'struct mldl_cfg'.
365 */
inv_get_dl_config(void)366 struct mldl_cfg *inv_get_dl_config(void)
367 {
368 return &mldlCfg;
369 }
370
371 /**
372 * @brief Query the MPU slave address.
373 * @return The 7-bit mpu slave address.
374 */
inv_get_mpu_slave_addr(void)375 unsigned char inv_get_mpu_slave_addr(void)
376 {
377 INVENSENSE_FUNC_START;
378 return mldlCfg.addr;
379 }
380
381 /**
382 * @internal
383 * @brief MLDLCfgDMP configures the Digital Motion Processor internal to
384 * the MPU. The DMP can be enabled or disabled and the start address
385 * can be set.
386 *
387 * @param enableRun Enables the DMP processing if set to TRUE.
388 * @param enableFIFO Enables DMP output to the FIFO if set to TRUE.
389 * @param startAddress start address
390 *
391 * @return Zero if the command is successful, an error code otherwise.
392 */
inv_get_dl_ctrl_dmp(unsigned char enableRun,unsigned char enableFIFO)393 inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
394 unsigned char enableFIFO)
395 {
396 INVENSENSE_FUNC_START;
397
398 mldlCfg.dmp_enable = enableRun;
399 mldlCfg.fifo_enable = enableFIFO;
400 mldlCfg.gyro_needs_reset = TRUE;
401
402 return INV_SUCCESS;
403 }
404
405 /**
406 * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin.
407 * The basic interrupt signal characteristics can be set
408 * (i.e. active high/low, open drain/push pull, etc.) and the
409 * triggers can be set.
410 * Currently only INTPIN_MPU is supported.
411 *
412 * @param triggers
413 * bitmask of triggers to enable for interrupt.
414 * The available triggers are:
415 * - BIT_MPU_RDY_EN
416 * - BIT_DMP_INT_EN
417 * - BIT_RAW_RDY_EN
418 *
419 * @return Zero if the command is successful, an error code otherwise.
420 */
inv_get_dl_cfg_int(unsigned char triggers)421 inv_error_t inv_get_dl_cfg_int(unsigned char triggers)
422 {
423 inv_error_t result = INV_SUCCESS;
424
425 #if defined CONFIG_MPU_SENSORS_MPU3050
426 /* Mantis has 8 bits of interrupt config bits */
427 if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) {
428 return INV_ERROR_INVALID_PARAMETER;
429 }
430 #endif
431
432 mldlCfg.int_config = triggers;
433 if (!mldlCfg.gyro_is_suspended) {
434 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
435 MPUREG_INT_CFG,
436 (mldlCfg.int_config | mldlCfg.pdata->
437 int_config));
438 } else {
439 mldlCfg.gyro_needs_reset = TRUE;
440 }
441
442 return result;
443 }
444
445 /**
446 * @brief configures the output sampling rate on the MPU.
447 * Three parameters control the sampling:
448 *
449 * 1) Low pass filter bandwidth, and
450 * 2) output sampling divider.
451 *
452 * The output sampling rate is determined by the divider and the low
453 * pass filter setting. If the low pass filter is set to
454 * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the
455 * divider is 8kHz; for all other settings it is 1kHz.
456 * The 8-bit divider will divide this frequency to get the resulting
457 * sample frequency.
458 * For example, if the filter setting is not 256Hz and the divider is
459 * set to 7, then the sample rate is as follows:
460 * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms).
461 *
462 * The low pass filter selection codes control both the cutoff frequency of
463 * the internal low pass filter and internal analog sampling rate. The
464 * latter, in turn, affects the final output sampling rate according to the
465 * sample rate divider settig.
466 * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate,
467 * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate,
468 * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate,
469 * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate,
470 * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate,
471 * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate,
472 * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate,
473 * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate.
474 *
475 * @param lpf low pass filter, 0 to 7.
476 * @param divider Output sampling rate divider, 0 to 255.
477 *
478 * @return ML_SUCESS if successful; a non-zero error code otherwise.
479 **/
inv_dl_cfg_sampling(unsigned char lpf,unsigned char divider)480 inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider)
481 {
482 /*---- do range checking ----*/
483 if (lpf >= NUM_MPU_FILTER) {
484 return INV_ERROR_INVALID_PARAMETER;
485 }
486
487 mldlCfg.lpf = lpf;
488 mldlCfg.divider = divider;
489 mldlCfg.gyro_needs_reset = TRUE;
490
491 return INV_SUCCESS;
492 }
493
494 /**
495 * @brief set the full scale range for the gyros.
496 * The full scale selection codes correspond to:
497 * 0 -> 250 dps,
498 * 1 -> 500 dps,
499 * 2 -> 1000 dps,
500 * 3 -> 2000 dps.
501 * Full scale range affect the MPU's measurement
502 * sensitivity.
503 *
504 * @param fullScale
505 * the gyro full scale range in dps.
506 *
507 * @return INV_SUCCESS or non-zero error code.
508 **/
inv_set_full_scale(float fullScale)509 inv_error_t inv_set_full_scale(float fullScale)
510 {
511 if (fullScale == 250.f)
512 mldlCfg.full_scale = MPU_FS_250DPS;
513 else if (fullScale == 500.f)
514 mldlCfg.full_scale = MPU_FS_500DPS;
515 else if (fullScale == 1000.f)
516 mldlCfg.full_scale = MPU_FS_1000DPS;
517 else if (fullScale == 2000.f)
518 mldlCfg.full_scale = MPU_FS_2000DPS;
519 else { // not a valid setting
520 MPL_LOGE("Invalid full scale range specification for gyros : %f\n",
521 fullScale);
522 MPL_LOGE
523 ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n");
524 return INV_ERROR_INVALID_PARAMETER;
525 }
526 mldlCfg.gyro_needs_reset = TRUE;
527
528 return INV_SUCCESS;
529 }
530
531 /**
532 * @brief This function sets the external sync for the MPU sampling.
533 * It can be synchronized on the LSB of any of the gyros, any of the
534 * external accels, or on the temp readings.
535 *
536 * @param extSync External sync selection, 0 to 7.
537 * @return Zero if the command is successful; an error code otherwise.
538 **/
inv_set_external_sync(unsigned char extSync)539 inv_error_t inv_set_external_sync(unsigned char extSync)
540 {
541 INVENSENSE_FUNC_START;
542
543 /*---- do range checking ----*/
544 if (extSync >= NUM_MPU_EXT_SYNC) {
545 return INV_ERROR_INVALID_PARAMETER;
546 }
547 mldlCfg.ext_sync = extSync;
548 mldlCfg.gyro_needs_reset = TRUE;
549
550 return INV_SUCCESS;
551 }
552
inv_set_ignore_system_suspend(unsigned char ignore)553 inv_error_t inv_set_ignore_system_suspend(unsigned char ignore)
554 {
555 INVENSENSE_FUNC_START;
556
557 mldlCfg.ignore_system_suspend = ignore;
558
559 return INV_SUCCESS;
560 }
561
562 /**
563 * @brief inv_clock_source function sets the clock source for the MPU gyro
564 * processing.
565 * The source can be any of the following:
566 * - Internal 8MHz oscillator,
567 * - PLL with X gyro as reference,
568 * - PLL with Y gyro as reference,
569 * - PLL with Z gyro as reference,
570 * - PLL with external 32.768Mhz reference, or
571 * - PLL with external 19.2MHz reference
572 *
573 * For best accuracy and timing, it is highly recommended to use one
574 * of the gyros as the clock source; however this gyro must be
575 * enabled to use its clock (see 'MLDLPowerMgmtMPU()').
576 *
577 * @param clkSource Clock source selection.
578 * Can be one of:
579 * - CLK_INTERNAL,
580 * - CLK_PLLGYROX,
581 * - CLK_PLLGYROY,
582 * - CLK_PLLGYROZ,
583 * - CLK_PLLEXT32K, or
584 * - CLK_PLLEXT19M.
585 *
586 * @return Zero if the command is successful; an error code otherwise.
587 **/
inv_clock_source(unsigned char clkSource)588 inv_error_t inv_clock_source(unsigned char clkSource)
589 {
590 INVENSENSE_FUNC_START;
591
592 /*---- do range checking ----*/
593 if (clkSource >= NUM_CLK_SEL) {
594 return INV_ERROR_INVALID_PARAMETER;
595 }
596
597 mldlCfg.clk_src = clkSource;
598 mldlCfg.gyro_needs_reset = TRUE;
599
600 return INV_SUCCESS;
601 }
602
603 /**
604 * @brief Set the Temperature Compensation offset.
605 * @param tc
606 * a pointer to the temperature compensations offset
607 * for the 3 gyro axes.
608 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
609 */
inv_set_offsetTC(const unsigned char * tc)610 inv_error_t inv_set_offsetTC(const unsigned char *tc)
611 {
612 int ii;
613 inv_error_t result;
614
615 for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) {
616 mldlCfg.offset_tc[ii] = tc[ii];
617 }
618
619 if (!mldlCfg.gyro_is_suspended) {
620 #ifdef CONFIG_MPU_SENSORS_MPU3050
621 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
622 MPUREG_XG_OFFS_TC, tc[0]);
623 if (result) {
624 LOG_RESULT_LOCATION(result);
625 return result;
626 }
627 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
628 MPUREG_YG_OFFS_TC, tc[1]);
629 if (result) {
630 LOG_RESULT_LOCATION(result);
631 return result;
632 }
633 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
634 MPUREG_ZG_OFFS_TC, tc[2]);
635 if (result) {
636 LOG_RESULT_LOCATION(result);
637 return result;
638 }
639 #else
640 unsigned char reg;
641 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
642 MPUREG_XG_OFFS_TC,
643 ((mldlCfg.offset_tc[0] << 1)
644 & BITS_XG_OFFS_TC));
645 if (result) {
646 LOG_RESULT_LOCATION(result);
647 return result;
648 }
649 reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC);
650 #ifdef CONFIG_MPU_SENSORS_MPU6050B1
651 if (mldlCfg.pdata->level_shifter)
652 reg |= BIT_I2C_MST_VDDIO;
653 #endif
654 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
655 MPUREG_YG_OFFS_TC, reg);
656 if (result) {
657 LOG_RESULT_LOCATION(result);
658 return result;
659 }
660 result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
661 MPUREG_ZG_OFFS_TC,
662 ((mldlCfg.offset_tc[2] << 1)
663 & BITS_ZG_OFFS_TC));
664 if (result) {
665 LOG_RESULT_LOCATION(result);
666 return result;
667 }
668 #endif
669 } else {
670 mldlCfg.gyro_needs_reset = TRUE;
671 }
672 return INV_SUCCESS;
673 }
674
675 /**
676 * @brief Set the gyro offset.
677 * @param offset
678 * a pointer to the gyro offset for the 3 gyro axes. This is scaled
679 * as it would be written to the hardware registers.
680 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
681 */
inv_set_offset(const short * offset)682 inv_error_t inv_set_offset(const short *offset)
683 {
684 inv_error_t result;
685 unsigned char regs[7];
686 int ii;
687 long sf;
688
689 sf = (2000L * 131) / mldlCfg.gyro_sens_trim;
690 for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) {
691 // Record the bias in the units the register uses
692 mldlCfg.offset[ii] = offset[ii];
693 // Convert the bias to 1 dps = 1<<16
694 inv_obj.gyro_bias[ii] = -offset[ii] * sf;
695 regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff;
696 regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff);
697 }
698
699 if (!mldlCfg.gyro_is_suspended) {
700 regs[0] = MPUREG_X_OFFS_USRH;
701 result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs);
702 if (result) {
703 LOG_RESULT_LOCATION(result);
704 return result;
705 }
706 } else {
707 mldlCfg.gyro_needs_reset = TRUE;
708 }
709 return INV_SUCCESS;
710 }
711
712 /**
713 * @internal
714 * @brief used to get the specified number of bytes in the specified MPU
715 * memory bank.
716 * The memory bank is one of the following:
717 * - MPUMEM_RAM_BANK_0,
718 * - MPUMEM_RAM_BANK_1,
719 * - MPUMEM_RAM_BANK_2, or
720 * - MPUMEM_RAM_BANK_3.
721 *
722 * @param bank Memory bank to write.
723 * @param memAddr Starting address for write.
724 * @param length Number of bytes to write.
725 * @param buffer Result for data.
726 *
727 * @return zero if the command is successful, an error code otherwise.
728 * @endif
729 */
730 inv_error_t
inv_get_mpu_memory_one_bank(unsigned char bank,unsigned char memAddr,unsigned short length,unsigned char * buffer)731 inv_get_mpu_memory_one_bank(unsigned char bank,
732 unsigned char memAddr,
733 unsigned short length, unsigned char *buffer)
734 {
735 inv_error_t result;
736
737 if ((bank >= MPU_MEM_NUM_RAM_BANKS) ||
738 //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255
739 ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
740 return INV_ERROR_INVALID_PARAMETER;
741 }
742
743 if (mldlCfg.gyro_is_suspended) {
744 memcpy(buffer, &mldlCfg.ram[bank][memAddr], length);
745 result = INV_SUCCESS;
746 } else {
747 result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr,
748 ((bank << 8) | memAddr), length, buffer);
749 if (result) {
750 LOG_RESULT_LOCATION(result);
751 return result;
752 }
753 }
754
755 return result;
756 }
757
758 /**
759 * @internal
760 * @brief used to set the specified number of bytes in the specified MPU
761 * memory bank.
762 * The memory bank is one of the following:
763 * - MPUMEM_RAM_BANK_0,
764 * - MPUMEM_RAM_BANK_1,
765 * - MPUMEM_RAM_BANK_2, or
766 * - MPUMEM_RAM_BANK_3.
767 *
768 * @param bank Memory bank to write.
769 * @param memAddr Starting address for write.
770 * @param length Number of bytes to write.
771 * @param buffer Result for data.
772 *
773 * @return zero if the command is successful, an error code otherwise.
774 * @endif
775 */
inv_set_mpu_memory_one_bank(unsigned char bank,unsigned short memAddr,unsigned short length,const unsigned char * buffer)776 inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank,
777 unsigned short memAddr,
778 unsigned short length,
779 const unsigned char *buffer)
780 {
781 inv_error_t result = INV_SUCCESS;
782 int different;
783
784 if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) ||
785 ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
786 return INV_ERROR_INVALID_PARAMETER;
787 }
788
789 different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length);
790 memcpy(&mldlCfg.ram[bank][memAddr], buffer, length);
791 if (!mldlCfg.gyro_is_suspended) {
792 result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr,
793 ((bank << 8) | memAddr), length, buffer);
794 if (result) {
795 LOG_RESULT_LOCATION(result);
796 return result;
797 }
798 } else if (different) {
799 mldlCfg.gyro_needs_reset = TRUE;
800 }
801
802 return result;
803 }
804
805 /**
806 * @internal
807 * @brief used to get the specified number of bytes from the MPU location
808 * specified by the key.
809 * Reads the specified number of bytes from the MPU location
810 * specified by the key. Each set of code specifies a function
811 * that changes keys into addresses. This function is set with
812 * setGetAddress().
813 *
814 * @param key The key to use when looking up the address.
815 * @param length Number of bytes to read.
816 * @param buffer Result for data.
817 *
818 * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
819 * not corresponding to a memory address will result in INV_ERROR.
820 * @endif
821 */
inv_get_mpu_memory(unsigned short key,unsigned short length,unsigned char * buffer)822 inv_error_t inv_get_mpu_memory(unsigned short key,
823 unsigned short length, unsigned char *buffer)
824 {
825 unsigned char bank;
826 inv_error_t result;
827 unsigned short memAddr;
828
829 if (sGetAddress == NULL) {
830 return INV_ERROR_NOT_OPENED;
831 }
832
833 memAddr = sGetAddress(key);
834 if (memAddr >= 0xffff)
835 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
836 bank = memAddr >> 8; // Get Bank
837 memAddr &= 0xff;
838
839 while (memAddr + length > MPU_MEM_BANK_SIZE) {
840 // We cross a bank in the middle
841 unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
842 result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
843 sub_length, buffer);
844 if (INV_SUCCESS != result)
845 return result;
846 bank++;
847 length -= sub_length;
848 buffer += sub_length;
849 memAddr = 0;
850 }
851 result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
852 length, buffer);
853
854 if (result) {
855 LOG_RESULT_LOCATION(result);
856 return result;
857 }
858
859 return result;
860 }
861
862 /**
863 * @internal
864 * @brief used to set the specified number of bytes from the MPU location
865 * specified by the key.
866 * Set the specified number of bytes from the MPU location
867 * specified by the key. Each set of DMP code specifies a function
868 * that changes keys into addresses. This function is set with
869 * setGetAddress().
870 *
871 * @param key The key to use when looking up the address.
872 * @param length Number of bytes to write.
873 * @param buffer Result for data.
874 *
875 * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
876 * not corresponding to a memory address will result in INV_ERROR.
877 * @endif
878 */
inv_set_mpu_memory(unsigned short key,unsigned short length,const unsigned char * buffer)879 inv_error_t inv_set_mpu_memory(unsigned short key,
880 unsigned short length,
881 const unsigned char *buffer)
882 {
883 inv_error_t result = INV_SUCCESS;
884 unsigned short memAddr;
885 unsigned char bank;
886
887 if (sGetAddress == NULL) {
888 MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n");
889 return INV_ERROR_INVALID_MODULE;
890 }
891 memAddr = sGetAddress(key);
892
893 if (memAddr >= 0xffff) {
894 MPL_LOGE("inv_set_mpu_memory unsupported key\n");
895 return INV_ERROR_INVALID_MODULE; // This key not supported
896 }
897
898 bank = (unsigned char)(memAddr >> 8);
899 memAddr &= 0xff;
900
901 while (memAddr + length > MPU_MEM_BANK_SIZE) {
902 // We cross a bank in the middle
903 unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
904
905 result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer);
906 if (result) {
907 LOG_RESULT_LOCATION(result);
908 return result;
909 }
910
911 bank++;
912 length -= sub_length;
913 buffer += sub_length;
914 memAddr = 0;
915 }
916 result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer);
917 if (result) {
918 LOG_RESULT_LOCATION(result);
919 return result;
920 }
921 return result;
922 }
923
924 /**
925 * @brief Load the DMP with the given code and configuration.
926 * @param buffer
927 * the DMP data.
928 * @param length
929 * the length in bytes of the DMP data.
930 * @param config
931 * the DMP configuration.
932 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
933 */
inv_load_dmp(const unsigned char * buffer,unsigned short length,unsigned short config)934 inv_error_t inv_load_dmp(const unsigned char *buffer,
935 unsigned short length, unsigned short config)
936 {
937 INVENSENSE_FUNC_START;
938
939 inv_error_t result = INV_SUCCESS;
940 unsigned short toWrite;
941 unsigned short memAddr = 0;
942 localDmpMemory = buffer;
943 localDmpMemorySize = length;
944
945 mldlCfg.dmp_cfg1 = (config >> 8);
946 mldlCfg.dmp_cfg2 = (config & 0xff);
947
948 while (length > 0) {
949 toWrite = length;
950 if (toWrite > MAX_LOAD_WRITE_SIZE)
951 toWrite = MAX_LOAD_WRITE_SIZE;
952
953 result =
954 inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite,
955 buffer);
956 if (result) {
957 LOG_RESULT_LOCATION(result);
958 return result;
959 }
960
961 buffer += toWrite;
962 memAddr += toWrite;
963 length -= toWrite;
964 }
965
966 return result;
967 }
968
969 /**
970 * @brief Get the silicon revision ID.
971 * @return The silicon revision ID
972 * (0 will be read if inv_mpu_open returned an error)
973 */
inv_get_silicon_rev(void)974 unsigned char inv_get_silicon_rev(void)
975 {
976 return mldlCfg.silicon_revision;
977 }
978
979 /**
980 * @brief Get the product revision ID.
981 * @return The product revision ID
982 * (0 will be read if inv_mpu_open returned an error)
983 */
inv_get_product_rev(void)984 unsigned char inv_get_product_rev(void)
985 {
986 return mldlCfg.product_revision;
987 }
988
989 /*******************************************************************************
990 *******************************************************************************
991 *******************************************************************************
992 * @todo these belong with an interface to the kernel driver layer
993 *******************************************************************************
994 *******************************************************************************
995 ******************************************************************************/
996
997 /**
998 * @brief inv_get_interrupt_status returns the interrupt status from the specified
999 * interrupt pin.
1000 * @param intPin
1001 * Currently only the value INTPIN_MPU is supported.
1002 * @param status
1003 * The available statuses are:
1004 * - BIT_MPU_RDY_EN
1005 * - BIT_DMP_INT_EN
1006 * - BIT_RAW_RDY_EN
1007 *
1008 * @return INV_SUCCESS or a non-zero error code.
1009 */
inv_get_interrupt_status(unsigned char intPin,unsigned char * status)1010 inv_error_t inv_get_interrupt_status(unsigned char intPin,
1011 unsigned char *status)
1012 {
1013 INVENSENSE_FUNC_START;
1014
1015 inv_error_t result;
1016
1017 switch (intPin) {
1018
1019 case INTPIN_MPU:
1020 /*---- return the MPU interrupt status ----*/
1021 result = inv_serial_read(sMLSLHandle, mldlCfg.addr,
1022 MPUREG_INT_STATUS, 1, status);
1023 break;
1024
1025 default:
1026 result = INV_ERROR_INVALID_PARAMETER;
1027 break;
1028 }
1029
1030 return result;
1031 }
1032
1033 /**
1034 * @brief query the current status of an interrupt source.
1035 * @param srcIndex
1036 * index of the interrupt source.
1037 * Currently the only source supported is INTPIN_MPU.
1038 *
1039 * @return 1 if the interrupt has been triggered.
1040 */
inv_get_interrupt_trigger(unsigned char srcIndex)1041 unsigned char inv_get_interrupt_trigger(unsigned char srcIndex)
1042 {
1043 INVENSENSE_FUNC_START;
1044 return intTrigger[srcIndex];
1045 }
1046
1047 /**
1048 * @brief clear the 'triggered' status for an interrupt source.
1049 * @param srcIndex
1050 * index of the interrupt source.
1051 * Currently only INTPIN_MPU is supported.
1052 */
inv_clear_interrupt_trigger(unsigned char srcIndex)1053 void inv_clear_interrupt_trigger(unsigned char srcIndex)
1054 {
1055 INVENSENSE_FUNC_START;
1056 intTrigger[srcIndex] = 0;
1057 }
1058
1059 /**
1060 * @brief inv_interrupt_handler function should be called when an interrupt is
1061 * received. The source parameter identifies which interrupt source
1062 * caused the interrupt. Note that this routine should not be called
1063 * directly from the interrupt service routine.
1064 *
1065 * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1,
1066 * INTSRC_AUX2, or INT_SRC_TIMER.
1067 *
1068 * @return Zero if the command is successful; an error code otherwise.
1069 */
inv_interrupt_handler(unsigned char intSource)1070 inv_error_t inv_interrupt_handler(unsigned char intSource)
1071 {
1072 INVENSENSE_FUNC_START;
1073 /*---- range check ----*/
1074 if (intSource >= NUM_OF_INTSOURCES) {
1075 return INV_ERROR;
1076 }
1077
1078 /*---- save source of interrupt ----*/
1079 intTrigger[intSource] = INT_TRIGGERED;
1080
1081 #ifdef ML_USE_DMP_SIM
1082 if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) {
1083 MLSimHWDataInput();
1084 }
1085 #endif
1086
1087 return INV_SUCCESS;
1088 }
1089
1090 /***************************/
1091 /**@}*//* end of defgroup */
1092 /***************************/
1093