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: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $
21 *
22 *******************************************************************************/
23
24 /**
25 * @defgroup MLFIFO
26 * @brief Motion Library - FIFO Driver.
27 * The FIFO API Interface.
28 *
29 * @{
30 * @file mlFIFO.c
31 * @brief FIFO Interface.
32 **/
33
34 #include <string.h>
35 #include "mpu.h"
36 #if defined CONFIG_MPU_SENSORS_MPU6050A2
37 # include "mpu6050a2.h"
38 #elif defined CONFIG_MPU_SENSORS_MPU6050B1
39 # include "mpu6050b1.h"
40 #elif defined CONFIG_MPU_SENSORS_MPU3050
41 # include "mpu3050.h"
42 #else
43 #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
44 #endif
45 #include "mlFIFO.h"
46 #include "mlFIFOHW.h"
47 #include "dmpKey.h"
48 #include "mlMathFunc.h"
49 #include "ml.h"
50 #include "mldl.h"
51 #include "mldl_cfg.h"
52 #include "mlstates.h"
53 #include "mlsupervisor.h"
54 #include "mlos.h"
55 #include "mlmath.h"
56 #include "accel.h"
57
58 #include "log.h"
59 #undef MPL_LOG_TAG
60 #define MPL_LOG_TAG "MPL-fifo"
61
62 #define FIFO_DEBUG 0
63
64 #define REF_QUATERNION (0)
65 #define REF_GYROS (REF_QUATERNION + 4)
66 #define REF_CONTROL (REF_GYROS + 3)
67 #define REF_RAW (REF_CONTROL + 4)
68 #define REF_RAW_EXTERNAL (REF_RAW + 8)
69 #define REF_ACCEL (REF_RAW_EXTERNAL + 6)
70 #define REF_QUANT_ACCEL (REF_ACCEL + 3)
71 #define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES)
72 #define REF_EIS (REF_QUATERNION_6AXIS + 4)
73 #define REF_DMP_PACKET (REF_EIS + 3)
74 #define REF_GARBAGE (REF_DMP_PACKET + 1)
75 #define REF_LAST (REF_GARBAGE + 1)
76
77 long fifo_scale[REF_LAST] = {
78 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion
79 // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2)
80 1501974482L, 1501974482L, 1501974482L, // Gyro
81 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control
82 (1L << 14), // Temperature
83 (1L << 14), (1L << 14), (1L << 14), // Raw Gyro
84 (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding
85 (1L << 14), (1L << 14), (1L << 14), // Raw External
86 (1L << 14), (1L << 14), (1L << 14), // Raw External
87 (1L << 16), (1L << 16), (1L << 16), // Accel
88 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel
89 (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel
90 (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis
91 (1L << 30), (1L << 30), (1L << 30), // EIS
92 (1L << 30), // Packet
93 (1L << 30), // Garbage
94 };
95
96 // The scale factors for tap need to match the number in fifo_scale above.
97 // fifo_base_offset below may also need to be changed if this is not 8
98 #if INV_MAX_NUM_ACCEL_SAMPLES != 8
99 #error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8
100 #endif
101
102 #define CONFIG_QUAT (0)
103 #define CONFIG_GYROS (CONFIG_QUAT + 1)
104 #define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1)
105 #define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1)
106 #define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1)
107 #define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1)
108 #define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1)
109 #define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1)
110 #define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1)
111 #define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1)
112 #define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1)
113 #define NUMFIFOELEMENTS (CONFIG_FOOTER + 1)
114
115 const int fifo_base_offset[NUMFIFOELEMENTS] = {
116 REF_QUATERNION * 4,
117 REF_GYROS * 4,
118 REF_CONTROL * 4,
119 REF_RAW * 4,
120 REF_RAW * 4 + 4,
121 REF_RAW_EXTERNAL * 4,
122 REF_ACCEL * 4,
123 REF_QUANT_ACCEL * 4,
124 REF_EIS * 4,
125 REF_DMP_PACKET * 4,
126 REF_GARBAGE * 4
127 };
128
129 struct fifo_obj {
130 void (*fifo_process_cb) (void);
131 long decoded[REF_LAST];
132 long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES];
133 int offsets[REF_LAST * 4];
134 int cache;
135 uint_fast8_t gyro_source;
136 unsigned short fifo_rate;
137 unsigned short sample_step_size_ms;
138 uint_fast16_t fifo_packet_size;
139 uint_fast16_t data_config[NUMFIFOELEMENTS];
140 unsigned char reference_count[REF_LAST];
141 long acc_bias_filt[3];
142 float acc_filter_coef;
143 long gravity_cache[3];
144 };
145
146 static struct fifo_obj fifo_obj;
147
148 #define FIFO_CACHE_TEMPERATURE 1
149 #define FIFO_CACHE_GYRO 2
150 #define FIFO_CACHE_GRAVITY_BODY 4
151 #define FIFO_CACHE_ACC_BIAS 8
152
153 struct fifo_rate_obj {
154 // These describe callbacks happening everytime a FIFO block is processed
155 int_fast8_t num_cb;
156 HANDLE mutex;
157 inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES];
158 int priority[MAX_HIGH_RATE_PROCESSES];
159 };
160
161 struct fifo_rate_obj fifo_rate_obj;
162
163 /** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old
164 * accuracy if needed.
165 */
inv_set_fifo_accuracy(uint_fast16_t elements,uint_fast16_t accuracy,uint_fast8_t configOffset)166 static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements,
167 uint_fast16_t accuracy,
168 uint_fast8_t configOffset)
169 {
170 if (elements) {
171 if (!accuracy)
172 accuracy = fifo_obj.data_config[configOffset];
173 else if (accuracy & INV_16_BIT)
174 if ((fifo_obj.data_config[configOffset] & INV_32_BIT))
175 accuracy = INV_32_BIT; // 32-bits takes priority
176 else
177 accuracy = INV_16_BIT;
178 else
179 accuracy = INV_32_BIT;
180 } else {
181 accuracy = 0;
182 }
183
184 return accuracy;
185 }
186
187 /** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0,
188 * the reference counts are subtracted, otherwise they are incremented for each
189 * bit set in element. The value returned are the elements that should be sent
190 * out as data through the FIFO.
191 */
inv_set_fifo_reference(uint_fast16_t elements,uint_fast16_t increment,uint_fast8_t refOffset,uint_fast8_t len)192 static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements,
193 uint_fast16_t increment,
194 uint_fast8_t refOffset,
195 uint_fast8_t len)
196 {
197 uint_fast8_t kk;
198
199 if (increment == 0) {
200 for (kk = 0; kk < len; ++kk) {
201 if ((elements & 1)
202 && (fifo_obj.reference_count[kk + refOffset] > 0)) {
203 fifo_obj.reference_count[kk + refOffset]--;
204 }
205 elements >>= 1;
206 }
207 } else {
208 for (kk = 0; kk < len; ++kk) {
209 if (elements & 1)
210 fifo_obj.reference_count[kk + refOffset]++;
211 elements >>= 1;
212 }
213 }
214 elements = 0;
215 for (kk = 0; kk < len; ++kk) {
216 if (fifo_obj.reference_count[kk + refOffset] > 0)
217 elements |= (1 << kk);
218 }
219 return elements;
220 }
221
222 /**
223 * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send
224 * out the FIFO, 0 when removing from the FIFO.
225 */
inv_construct3_fifo(unsigned char * regs,uint_fast16_t elements,uint_fast16_t accuracy,uint_fast8_t refOffset,unsigned short key,uint_fast8_t configOffset)226 static inv_error_t inv_construct3_fifo(unsigned char *regs,
227 uint_fast16_t elements,
228 uint_fast16_t accuracy,
229 uint_fast8_t refOffset,
230 unsigned short key,
231 uint_fast8_t configOffset)
232 {
233 int_fast8_t kk;
234 inv_error_t result;
235
236 elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3);
237 accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset);
238
239 if (accuracy & INV_16_BIT) {
240 regs[0] = DINAF8 + 2;
241 }
242
243 fifo_obj.data_config[configOffset] = elements | accuracy;
244
245 for (kk = 0; kk < 3; ++kk) {
246 if ((elements & 1) == 0)
247 regs[kk + 1] = DINAA0 + 3;
248 elements >>= 1;
249 }
250
251 result = inv_set_mpu_memory(key, 4, regs);
252
253 return result;
254 }
255
256 /**
257 * @internal
258 * Puts footer on FIFO data.
259 */
inv_set_footer(void)260 static inv_error_t inv_set_footer(void)
261 {
262 unsigned char regs = DINA30;
263 uint_fast8_t tmp_count;
264 int_fast8_t i, j;
265 int offset;
266 int result;
267 int *fifo_offsets_ptr = fifo_obj.offsets;
268
269 fifo_obj.fifo_packet_size = 0;
270 for (i = 0; i < NUMFIFOELEMENTS; i++) {
271 tmp_count = 0;
272 offset = fifo_base_offset[i];
273 for (j = 0; j < 8; j++) {
274 if ((fifo_obj.data_config[i] >> j) & 0x0001) {
275 #ifndef BIG_ENDIAN
276 // Special Case for Byte Ordering on Accel Data
277 if ((i == CONFIG_RAW_DATA) && (j > 2)) {
278 tmp_count += 2;
279 switch (inv_get_dl_config()->accel->endian) {
280 case EXT_SLAVE_BIG_ENDIAN:
281 *fifo_offsets_ptr++ = offset + 3;
282 *fifo_offsets_ptr++ = offset + 2;
283 break;
284 case EXT_SLAVE_LITTLE_ENDIAN:
285 *fifo_offsets_ptr++ = offset + 2;
286 *fifo_offsets_ptr++ = offset + 3;
287 break;
288 case EXT_SLAVE_FS8_BIG_ENDIAN:
289 if (j == 3) {
290 // Throw this byte away
291 *fifo_offsets_ptr++ =
292 fifo_base_offset[CONFIG_FOOTER];
293 *fifo_offsets_ptr++ = offset + 3;
294 } else if (j == 4) {
295 *fifo_offsets_ptr++ = offset + 3;
296 *fifo_offsets_ptr++ = offset + 7;
297 } else {
298 // Throw these byte away
299 *fifo_offsets_ptr++ =
300 fifo_base_offset[CONFIG_FOOTER];
301 *fifo_offsets_ptr++ =
302 fifo_base_offset[CONFIG_FOOTER];
303 }
304 break;
305 case EXT_SLAVE_FS16_BIG_ENDIAN:
306 if (j == 3) {
307 // Throw this byte away
308 *fifo_offsets_ptr++ =
309 fifo_base_offset[CONFIG_FOOTER];
310 *fifo_offsets_ptr++ = offset + 3;
311 } else if (j == 4) {
312 *fifo_offsets_ptr++ = offset - 2;
313 *fifo_offsets_ptr++ = offset + 3;
314 } else {
315 *fifo_offsets_ptr++ = offset - 2;
316 *fifo_offsets_ptr++ = offset + 3;
317 }
318 break;
319 default:
320 return INV_ERROR; // Bad value on ordering
321 }
322 } else {
323 tmp_count += 2;
324 *fifo_offsets_ptr++ = offset + 3;
325 *fifo_offsets_ptr++ = offset + 2;
326 if (fifo_obj.data_config[i] & INV_32_BIT) {
327 *fifo_offsets_ptr++ = offset + 1;
328 *fifo_offsets_ptr++ = offset;
329 tmp_count += 2;
330 }
331 }
332 #else
333 // Big Endian Platform
334 // Special Case for Byte Ordering on Accel Data
335 if ((i == CONFIG_RAW_DATA) && (j > 2)) {
336 tmp_count += 2;
337 switch (inv_get_dl_config()->accel->endian) {
338 case EXT_SLAVE_BIG_ENDIAN:
339 *fifo_offsets_ptr++ = offset + 2;
340 *fifo_offsets_ptr++ = offset + 3;
341 break;
342 case EXT_SLAVE_LITTLE_ENDIAN:
343 *fifo_offsets_ptr++ = offset + 3;
344 *fifo_offsets_ptr++ = offset + 2;
345 break;
346 case EXT_SLAVE_FS8_BIG_ENDIAN:
347 if (j == 3) {
348 // Throw this byte away
349 *fifo_offsets_ptr++ =
350 fifo_base_offset[CONFIG_FOOTER];
351 *fifo_offsets_ptr++ = offset;
352 } else if (j == 4) {
353 *fifo_offsets_ptr++ = offset;
354 *fifo_offsets_ptr++ = offset + 4;
355 } else {
356 // Throw these bytes away
357 *fifo_offsets_ptr++ =
358 fifo_base_offset[CONFIG_FOOTER];
359 *fifo_offsets_ptr++ =
360 fifo_base_offset[CONFIG_FOOTER];
361 }
362 break;
363 case EXT_SLAVE_FS16_BIG_ENDIAN:
364 if (j == 3) {
365 // Throw this byte away
366 *fifo_offsets_ptr++ =
367 fifo_base_offset[CONFIG_FOOTER];
368 *fifo_offsets_ptr++ = offset;
369 } else if (j == 4) {
370 *fifo_offsets_ptr++ = offset - 3;
371 *fifo_offsets_ptr++ = offset;
372 } else {
373 *fifo_offsets_ptr++ = offset - 3;
374 *fifo_offsets_ptr++ = offset;
375 }
376 break;
377 default:
378 return INV_ERROR; // Bad value on ordering
379 }
380 } else {
381 tmp_count += 2;
382 *fifo_offsets_ptr++ = offset;
383 *fifo_offsets_ptr++ = offset + 1;
384 if (fifo_obj.data_config[i] & INV_32_BIT) {
385 *fifo_offsets_ptr++ = offset + 2;
386 *fifo_offsets_ptr++ = offset + 3;
387 tmp_count += 2;
388 }
389 }
390
391 #endif
392 }
393 offset += 4;
394 }
395 fifo_obj.fifo_packet_size += tmp_count;
396 }
397 if (fifo_obj.data_config[CONFIG_FOOTER] == 0 &&
398 fifo_obj.fifo_packet_size > 0) {
399 // Add footer
400 result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s);
401 if (result) {
402 LOG_RESULT_LOCATION(result);
403 return result;
404 }
405 fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT;
406 fifo_obj.fifo_packet_size += 2;
407 } else if (fifo_obj.data_config[CONFIG_FOOTER] &&
408 (fifo_obj.fifo_packet_size == 2)) {
409 // Remove Footer
410 regs = DINAA0 + 3;
411 result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s);
412 if (result) {
413 LOG_RESULT_LOCATION(result);
414 return result;
415 }
416 fifo_obj.data_config[CONFIG_FOOTER] = 0;
417 fifo_obj.fifo_packet_size = 0;
418 }
419
420 return INV_SUCCESS;
421 }
422
inv_decode_quantized_accel(void)423 inv_error_t inv_decode_quantized_accel(void)
424 {
425 int kk;
426 int fifoRate = inv_get_fifo_rate();
427
428 if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
429 return INV_ERROR_FEATURE_NOT_ENABLED;
430
431 for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1));
432 kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) {
433 union {
434 unsigned int u10:10;
435 signed int s10:10;
436 } temp;
437
438 union {
439 uint32_t u32;
440 int32_t s32;
441 } value;
442
443 value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk];
444 // unquantize this samples.
445 // They are stored as x * 2^20 + y * 2^10 + z
446 // Z
447 temp.u10 = value.u32 & 0x3ff;
448 value.s32 -= temp.s10;
449 fifo_obj.decoded_accel[kk][2] = temp.s10 * 64;
450 // Y
451 value.s32 = value.s32 / 1024;
452 temp.u10 = value.u32 & 0x3ff;
453 value.s32 -= temp.s10;
454 fifo_obj.decoded_accel[kk][1] = temp.s10 * 64;
455 // X
456 value.s32 = value.s32 / 1024;
457 temp.u10 = value.u32 & 0x3ff;
458 fifo_obj.decoded_accel[kk][0] = temp.s10 * 64;
459 }
460 return INV_SUCCESS;
461 }
462
inv_state_change_fifo(unsigned char newState)463 static inv_error_t inv_state_change_fifo(unsigned char newState)
464 {
465 inv_error_t result = INV_SUCCESS;
466 unsigned char regs[4];
467 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
468
469 /* Don't reset the fifo on a fifo rate change */
470 if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
471 (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) {
472 /* Delay output on restart by 50ms due to warm up time of gyros */
473
474 short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1);
475 inv_init_fifo_hardare();
476 inv_int16_to_big8(delay, regs);
477 result = inv_set_mpu_memory(KEY_D_1_178, 2, regs);
478 if (result) {
479 LOG_RESULT_LOCATION(result);
480 return result;
481 }
482 }
483
484 if (INV_STATE_DMP_STARTED == newState) {
485 if (inv_dmpkey_supported(KEY_D_1_128)) {
486 double tmp;
487 tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1);
488 if (tmp > 0x40000000L)
489 tmp = 0x40000000L;
490 (void)inv_int32_to_big8((long)tmp, regs);
491 result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs);
492 if (result) {
493 LOG_RESULT_LOCATION(result);
494 return result;
495 }
496 result = inv_reset_fifo();
497 if (result) {
498 LOG_RESULT_LOCATION(result);
499 return result;
500 }
501 }
502 }
503 return result;
504 }
505
506 /**
507 * @internal
508 * @brief get the FIFO packet size
509 * @return the FIFO packet size
510 */
inv_get_fifo_packet_size(void)511 uint_fast16_t inv_get_fifo_packet_size(void)
512 {
513 return fifo_obj.fifo_packet_size;
514 }
515
516 /**
517 * @brief Initializes all the internal static variables for
518 * the FIFO module.
519 * @note Should be called by the initialization routine such
520 * as inv_dmp_open().
521 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
522 */
inv_init_fifo_param(void)523 inv_error_t inv_init_fifo_param(void)
524 {
525 inv_error_t result;
526 memset(&fifo_obj, 0, sizeof(struct fifo_obj));
527 fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity
528 inv_set_linear_accel_filter_coef(0.f);
529 fifo_obj.fifo_rate = 20;
530 fifo_obj.sample_step_size_ms = 100;
531 memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
532 result = inv_create_mutex(&fifo_rate_obj.mutex);
533 if (result) {
534 LOG_RESULT_LOCATION(result);
535 return result;
536 }
537 result = inv_register_state_callback(inv_state_change_fifo);
538 if (result) {
539 LOG_RESULT_LOCATION(result);
540 return result;
541 }
542 return result;
543 }
544
545 /**
546 * @brief Close the FIFO usage.
547 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
548 */
inv_close_fifo(void)549 inv_error_t inv_close_fifo(void)
550 {
551 inv_error_t result;
552 inv_error_t first = INV_SUCCESS;
553 result = inv_unregister_state_callback(inv_state_change_fifo);
554 ERROR_CHECK_FIRST(first, result);
555 result = inv_destroy_mutex(fifo_rate_obj.mutex);
556 ERROR_CHECK_FIRST(first, result);
557 memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
558 return first;
559 }
560
561 /**
562 * Set the gyro source to output to the fifo
563 *
564 * @param source The source. One of
565 * - INV_GYRO_FROM_RAW
566 * - INV_GYRO_FROM_QUATERNION
567 *
568 * @return INV_SUCCESS or non-zero error code;
569 */
inv_set_gyro_data_source(uint_fast8_t source)570 inv_error_t inv_set_gyro_data_source(uint_fast8_t source)
571 {
572 if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) {
573 return INV_ERROR_INVALID_PARAMETER;
574 }
575
576 fifo_obj.gyro_source = source;
577 return INV_SUCCESS;
578 }
579
580 /**
581 * @brief Reads and processes FIFO data. Also handles callbacks when data is
582 * ready.
583 * @param numPackets
584 * Number of FIFO packets to try to read. You should
585 * use a large number here, such as 100, if you want to read all
586 * the full packets in the FIFO, which is typical operation.
587 * @param processed
588 * The number of FIFO packets processed. This may be incremented
589 * even if high rate processes later fail.
590 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
591 */
inv_read_and_process_fifo(int_fast8_t numPackets,int_fast8_t * processed)592 inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets,
593 int_fast8_t * processed)
594 {
595 int_fast8_t packet;
596 inv_error_t result = INV_SUCCESS;
597 uint_fast16_t read;
598 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
599 int kk;
600
601 if (NULL == processed)
602 return INV_ERROR_INVALID_PARAMETER;
603
604 *processed = 0;
605 if (fifo_obj.fifo_packet_size == 0)
606 return result; // Nothing to read
607
608 for (packet = 0; packet < numPackets; ++packet) {
609 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
610 unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE];
611 unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE];
612 read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size,
613 footer_n_data);
614 if (0 == read ||
615 read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) {
616 result = inv_get_fifo_status();
617 if (INV_SUCCESS != result) {
618 memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
619 }
620 return result;
621 }
622
623 result = inv_process_fifo_packet(buf);
624 if (result) {
625 LOG_RESULT_LOCATION(result);
626 return result;
627 }
628 } else if (inv_accel_present()) {
629 long data[ACCEL_NUM_AXES];
630 result = inv_get_accel_data(data);
631 if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
632 return INV_SUCCESS;
633 }
634 if (result) {
635 LOG_RESULT_LOCATION(result);
636 return result;
637 }
638
639 memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
640 fifo_obj.cache = 0;
641 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
642 fifo_obj.decoded[REF_RAW + 4 + kk] =
643 inv_q30_mult((data[kk] << 16),
644 fifo_scale[REF_RAW + 4 + kk]);
645 fifo_obj.decoded[REF_ACCEL + kk] =
646 inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]);
647 fifo_obj.decoded[REF_ACCEL + kk] -=
648 inv_obj.scaled_accel_bias[kk];
649 }
650 }
651 // The buffer was processed correctly, so increment even if
652 // other processes fail later, which will return an error
653 *processed = *processed + 1;
654
655 if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) &&
656 fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) {
657 result = inv_decode_quantized_accel();
658 if (result) {
659 LOG_RESULT_LOCATION(result);
660 return result;
661 }
662 }
663
664 if (fifo_obj.data_config[CONFIG_QUAT]) {
665 result = inv_accel_compass_supervisor();
666 if (result) {
667 LOG_RESULT_LOCATION(result);
668 return result;
669 }
670 }
671
672 result = inv_pressure_supervisor();
673 if (result) {
674 LOG_RESULT_LOCATION(result);
675 return result;
676 }
677
678 // Callbacks now that we have a buffer of data ready
679 result = inv_run_fifo_rate_processes();
680 if (result) {
681 LOG_RESULT_LOCATION(result);
682 return result;
683 }
684
685 }
686 return result;
687 }
688
689 /**
690 * @brief inv_set_fifo_processed_callback is used to set a processed data callback
691 * function. inv_set_fifo_processed_callback sets a user defined callback
692 * function that triggers when all the decoding has been finished by
693 * the motion processing engines. It is called before other bigger
694 * processing engines to allow lower latency for the user.
695 *
696 * @pre inv_dmp_open()
697 * @ifnot MPL_MF
698 * or inv_open_low_power_pedometer()
699 * or inv_eis_open_dmp()
700 * @endif
701 * and inv_dmp_start()
702 * must <b>NOT</b> have been called.
703 *
704 * @param func A user defined callback function.
705 *
706 * @return INV_SUCCESS if successful, or non-zero error code otherwise.
707 */
inv_set_fifo_processed_callback(void (* func)(void))708 inv_error_t inv_set_fifo_processed_callback(void (*func) (void))
709 {
710 INVENSENSE_FUNC_START;
711
712 if (inv_get_state() < INV_STATE_DMP_OPENED)
713 return INV_ERROR_SM_IMPROPER_STATE;
714
715 fifo_obj.fifo_process_cb = func;
716
717 return INV_SUCCESS;
718 }
719
720 /**
721 * @internal
722 * @brief Process data from the dmp read via the fifo. Takes a buffer
723 * filled with bytes read from the DMP FIFO.
724 * Currently expects only the 16 bytes of quaternion data.
725 * Calculates the motion parameters from that data and stores the
726 * results in an internal data structure.
727 * @param[in,out] dmpData Pointer to the buffer containing the fifo data.
728 * @return INV_SUCCESS or error code.
729 **/
inv_process_fifo_packet(const unsigned char * dmpData)730 inv_error_t inv_process_fifo_packet(const unsigned char *dmpData)
731 {
732 INVENSENSE_FUNC_START;
733 int N, kk;
734 unsigned char *p;
735
736 p = (unsigned char *)(&fifo_obj.decoded);
737 N = fifo_obj.fifo_packet_size;
738 if (N > sizeof(fifo_obj.decoded))
739 return INV_ERROR_ASSERTION_FAILURE;
740
741 memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
742
743 for (kk = 0; kk < N; ++kk) {
744 p[fifo_obj.offsets[kk]] = *dmpData++;
745 }
746
747 // If multiplies are much greater cost than if checks, you could check
748 // to see if fifo_scale is non-zero first, or equal to (1L<<30)
749 for (kk = 0; kk < REF_LAST; ++kk) {
750 fifo_obj.decoded[kk] =
751 inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]);
752 }
753
754 memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS],
755 &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long));
756
757 inv_obj.flags[INV_PROCESSED_DATA_READY] = 1;
758 fifo_obj.cache = 0;
759
760 return INV_SUCCESS;
761 }
762
763 /** Converts 16-bit temperature data as read from temperature register
764 * into Celcius scaled by 2^16.
765 */
inv_decode_temperature(short tempReg)766 long inv_decode_temperature(short tempReg)
767 {
768 #if defined CONFIG_MPU_SENSORS_MPU6050A2
769 // Celcius = 35 + (T + 3048.7)/325.9
770 return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L);
771 #endif
772 #if defined CONFIG_MPU_SENSORS_MPU6050B1
773 // Celcius = 35 + (T + 927.4)/360.6
774 return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L);
775 #endif
776 #if defined CONFIG_MPU_SENSORS_MPU3050
777 // Celcius = 35 + (T + 13200)/280
778 return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L);
779 #endif
780 }
781
782 /** @internal
783 * Returns the temperature in hardware units. The scaling may change from part to part.
784 */
inv_get_temperature_raw(short * data)785 inv_error_t inv_get_temperature_raw(short *data)
786 {
787 if (data == NULL)
788 return INV_ERROR_INVALID_PARAMETER;
789
790 if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) {
791 inv_error_t result;
792 unsigned char regs[2];
793 if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) {
794 if (FIFO_DEBUG)
795 MPL_LOGI("Fetching the temperature from the registers\n");
796 fifo_obj.cache |= FIFO_CACHE_TEMPERATURE;
797 result = inv_serial_read(inv_get_serial_handle(),
798 inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2,
799 regs);
800 if (result) {
801 LOG_RESULT_LOCATION(result);
802 return result;
803 }
804 fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]);
805 }
806 }
807 *data = (short)fifo_obj.decoded[REF_RAW];
808 return INV_SUCCESS;
809 }
810
811 /**
812 * @brief Returns 1-element vector of temperature. It is read from the hardware if it
813 * doesn't exist in the FIFO.
814 * @param[out] data 1-element vector of temperature
815 * @return 0 on success or an error code.
816 */
inv_get_temperature(long * data)817 inv_error_t inv_get_temperature(long *data)
818 {
819 short tr;
820 inv_error_t result;
821
822 if (data == NULL)
823 return INV_ERROR_INVALID_PARAMETER;
824 result = inv_get_temperature_raw(&tr);
825 if (result) {
826 LOG_RESULT_LOCATION(result);
827 return result;
828 }
829 data[0] = inv_decode_temperature(tr);
830 return INV_SUCCESS;
831 }
832
833 /**
834 * @brief Get the Decoded Accel Data.
835 * @param data
836 * a buffer to store the quantized data.
837 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
838 */
inv_get_unquantized_accel(long * data)839 inv_error_t inv_get_unquantized_accel(long *data)
840 {
841 int ii, kk;
842 if (data == NULL)
843 return INV_ERROR_INVALID_PARAMETER;
844
845 if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
846 return INV_ERROR_FEATURE_NOT_ENABLED;
847
848 for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
849 for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
850 data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk];
851 }
852 }
853
854 return INV_SUCCESS;
855 }
856
857 /**
858 * @brief Get the Quantized Accel data algorithm output from the FIFO.
859 * @param data
860 * a buffer to store the quantized data.
861 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
862 */
inv_get_quantized_accel(long * data)863 inv_error_t inv_get_quantized_accel(long *data)
864 {
865 int ii;
866 if (data == NULL)
867 return INV_ERROR_INVALID_PARAMETER;
868
869 if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
870 return INV_ERROR_FEATURE_NOT_ENABLED;
871
872 for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
873 data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii];
874 }
875
876 return INV_SUCCESS;
877 }
878
879 /** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO
880 * and it is read from the registers if it was not put into the FIFO. The data is
881 * cached till the next FIFO processing block time.
882 * @param[out] data Length 3, Gyro data
883 */
inv_get_gyro_sensor(long * data)884 inv_error_t inv_get_gyro_sensor(long *data)
885 {
886 if (data == NULL)
887 return INV_ERROR_INVALID_PARAMETER;
888 if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) {
889 inv_error_t result;
890 unsigned char regs[6];
891 if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) {
892 fifo_obj.cache |= FIFO_CACHE_GYRO;
893 result =
894 inv_serial_read(inv_get_serial_handle(),
895 inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6,
896 regs);
897 if (result) {
898 LOG_RESULT_LOCATION(result);
899 return result;
900 }
901 fifo_obj.decoded[REF_RAW + 1] =
902 (((long)regs[0]) << 24) | (((long)regs[1]) << 16);
903 fifo_obj.decoded[REF_RAW + 2] =
904 (((long)regs[2]) << 24) | (((long)regs[3]) << 16);
905 fifo_obj.decoded[REF_RAW + 3] =
906 (((long)regs[4]) << 24) | (((long)regs[5]) << 16);
907
908 // Temperature starts at location 0, Gyro at location 1.
909 fifo_obj.decoded[REF_RAW + 1] =
910 inv_q30_mult(fifo_obj.decoded[REF_RAW + 1],
911 fifo_scale[REF_RAW + 1]);
912 fifo_obj.decoded[REF_RAW + 2] =
913 inv_q30_mult(fifo_obj.decoded[REF_RAW + 2],
914 fifo_scale[REF_RAW + 2]);
915 fifo_obj.decoded[REF_RAW + 3] =
916 inv_q30_mult(fifo_obj.decoded[REF_RAW + 3],
917 fifo_scale[REF_RAW + 3]);
918 }
919 data[0] = fifo_obj.decoded[REF_RAW + 1];
920 data[1] = fifo_obj.decoded[REF_RAW + 2];
921 data[2] = fifo_obj.decoded[REF_RAW + 3];
922 } else {
923 long data2[6];
924 inv_get_gyro_and_accel_sensor(data2);
925 data[0] = data2[0];
926 data[1] = data2[1];
927 data[2] = data2[2];
928 }
929 return INV_SUCCESS;
930 }
931
932 /**
933 * @brief Returns 6-element vector of gyro and accel data
934 * @param[out] data 6-element vector of gyro and accel data
935 * @return 0 on success or an error code.
936 */
inv_get_gyro_and_accel_sensor(long * data)937 inv_error_t inv_get_gyro_and_accel_sensor(long *data)
938 {
939 int ii;
940 if (data == NULL)
941 return INV_ERROR_INVALID_PARAMETER;
942
943 if (!fifo_obj.data_config[CONFIG_RAW_DATA])
944 return INV_ERROR_FEATURE_NOT_ENABLED;
945
946 for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) {
947 data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii];
948 }
949
950 return INV_SUCCESS;
951 }
952
953 /**
954 * @brief Returns 3-element vector of external sensor
955 * @param[out] data 3-element vector of external sensor
956 * @return 0 on success or an error code.
957 */
inv_get_external_sensor_data(long * data,int size)958 inv_error_t inv_get_external_sensor_data(long *data, int size)
959 {
960 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
961 defined CONFIG_MPU_SENSORS_MPU6050B1
962 int ii;
963 if (data == NULL)
964 return INV_ERROR_INVALID_PARAMETER;
965
966 if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL])
967 return INV_ERROR_FEATURE_NOT_ENABLED;
968
969 for (ii = 0; ii < size && ii < 6; ii++) {
970 data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii];
971 }
972
973 return INV_SUCCESS;
974 #else
975 memset(data, 0, COMPASS_NUM_AXES * sizeof(long));
976 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
977 #endif
978 }
979
980 /**
981 * Sends accelerometer data to the FIFO.
982 *
983 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis
984 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
985 * for a subset.
986 *
987 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
988 * bit data. Set to zero to remove it from the FIFO.
989 */
inv_send_accel(uint_fast16_t elements,uint_fast16_t accuracy)990 inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy)
991 {
992 INVENSENSE_FUNC_START;
993 unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 };
994 inv_error_t result;
995 int kk;
996
997 if (inv_get_state() < INV_STATE_DMP_OPENED)
998 return INV_ERROR_SM_IMPROPER_STATE;
999
1000 result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL,
1001 KEY_CFG_12, CONFIG_ACCEL);
1002 if (result) {
1003 LOG_RESULT_LOCATION(result);
1004 return result;
1005 }
1006
1007 for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
1008 fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens;
1009 }
1010
1011 return inv_set_footer();
1012 }
1013
1014 /**
1015 * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits.
1016 *
1017 * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all
1018 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd
1019 * together for a subset.
1020 *
1021 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
1022 * bit data. Set to zero to remove it from the FIFO.
1023 */
inv_send_cntrl_data(uint_fast16_t elements,uint_fast16_t accuracy)1024 inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy)
1025 {
1026 INVENSENSE_FUNC_START;
1027 int_fast8_t kk;
1028 inv_error_t result;
1029 unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 };
1030
1031 if (inv_get_state() < INV_STATE_DMP_OPENED)
1032 return INV_ERROR_SM_IMPROPER_STATE;
1033
1034 elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4);
1035 accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA);
1036
1037 if (accuracy & INV_16_BIT) {
1038 regs[0] = DINAF8 + 2;
1039 }
1040
1041 fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy;
1042
1043 for (kk = 0; kk < 4; ++kk) {
1044 if ((elements & 1) == 0)
1045 regs[kk + 1] = DINAA0 + 3;
1046 elements >>= 1;
1047 }
1048
1049 result = inv_set_mpu_memory(KEY_CFG_1, 5, regs);
1050 if (result) {
1051 LOG_RESULT_LOCATION(result);
1052 return result;
1053 }
1054
1055 return inv_set_footer();
1056 }
1057
1058 /**
1059 * Adds a rolling counter to the fifo packet. When used with the footer
1060 * the data comes out the first time:
1061 *
1062 * @code
1063 * <data0><data1>...<dataN><PacketNum0><PacketNum1>
1064 * @endcode
1065 * for every other packet it is
1066 *
1067 * @code
1068 * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1>
1069 * @endcode
1070 *
1071 * This allows for scanning of the fifo for packets
1072 *
1073 * @return INV_SUCCESS or error code
1074 */
inv_send_packet_number(uint_fast16_t accuracy)1075 inv_error_t inv_send_packet_number(uint_fast16_t accuracy)
1076 {
1077 INVENSENSE_FUNC_START;
1078 inv_error_t result;
1079 unsigned char regs;
1080 uint_fast16_t elements;
1081
1082 if (inv_get_state() < INV_STATE_DMP_OPENED)
1083 return INV_ERROR_SM_IMPROPER_STATE;
1084
1085 elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1);
1086 if (elements & 1) {
1087 regs = DINA28;
1088 fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] =
1089 INV_ELEMENT_1 | INV_16_BIT;
1090 } else {
1091 regs = DINAF8 + 3;
1092 fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0;
1093 }
1094 result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s);
1095 if (result) {
1096 LOG_RESULT_LOCATION(result);
1097 return result;
1098 }
1099
1100 return inv_set_footer();
1101 }
1102
1103 /**
1104 * @brief Send the computed gravity vectors into the FIFO.
1105 * The gravity vectors can be retrieved from the FIFO via
1106 * inv_get_gravity(), to have the gravitation vector expressed
1107 * in coordinates relative to the body.
1108 *
1109 * Gravity is a derived vector derived from the quaternion.
1110 * @param elements
1111 * the gravitation vectors components bitmask.
1112 * To send all compoents use INV_ALL.
1113 * @param accuracy
1114 * The number of bits the gravitation vector is expressed
1115 * into.
1116 * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
1117 * bit data.
1118 * Set to zero to remove it from the FIFO.
1119 *
1120 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
1121 */
inv_send_gravity(uint_fast16_t elements,uint_fast16_t accuracy)1122 inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy)
1123 {
1124 INVENSENSE_FUNC_START;
1125 inv_error_t result;
1126
1127 result = inv_send_quaternion(accuracy);
1128 if (result) {
1129 LOG_RESULT_LOCATION(result);
1130 return result;
1131 }
1132
1133 return inv_set_footer();
1134 }
1135
1136 /** Sends gyro data to the FIFO. Gyro data is a 3 length vector
1137 * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
1138 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
1139 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
1140 * for a subset.
1141 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
1142 * bit data. Set to zero to remove it from the FIFO.
1143 */
inv_send_gyro(uint_fast16_t elements,uint_fast16_t accuracy)1144 inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy)
1145 {
1146 INVENSENSE_FUNC_START;
1147 unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 };
1148 inv_error_t result;
1149
1150 if (inv_get_state() < INV_STATE_DMP_OPENED)
1151 return INV_ERROR_SM_IMPROPER_STATE;
1152
1153 if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) {
1154 regs[0] = DINA90 + 5;
1155 result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
1156 if (result) {
1157 LOG_RESULT_LOCATION(result);
1158 return result;
1159 }
1160 regs[0] = DINAF8 + 1;
1161 regs[1] = DINA20;
1162 regs[2] = DINA28;
1163 regs[3] = DINA30;
1164 } else {
1165 regs[0] = DINA90 + 10;
1166 result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
1167 if (result) {
1168 LOG_RESULT_LOCATION(result);
1169 return result;
1170 }
1171 regs[0] = DINAF8 + 1;
1172 regs[1] = DINA28;
1173 regs[2] = DINA30;
1174 regs[3] = DINA38;
1175 }
1176 result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS,
1177 KEY_CFG_9, CONFIG_GYROS);
1178
1179 return inv_set_footer();
1180 }
1181
1182 /** Sends linear accelerometer data to the FIFO.
1183 *
1184 * Linear accelerometer data is a 3 length vector of 32-bits. It is the
1185 * acceleration in the body frame with gravity removed.
1186 *
1187 *
1188 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of
1189 * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
1190 * for a subset.
1191 *
1192 * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES
1193 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
1194 * bit data. Set to zero to remove it from the FIFO.
1195 */
inv_send_linear_accel(uint_fast16_t elements,uint_fast16_t accuracy)1196 inv_error_t inv_send_linear_accel(uint_fast16_t elements,
1197 uint_fast16_t accuracy)
1198 {
1199 INVENSENSE_FUNC_START;
1200 inv_error_t result;
1201 unsigned char state = inv_get_state();
1202
1203 if (state < INV_STATE_DMP_OPENED)
1204 return INV_ERROR_SM_IMPROPER_STATE;
1205
1206 result = inv_send_gravity(elements, accuracy);
1207 if (result) {
1208 LOG_RESULT_LOCATION(result);
1209 return result;
1210 }
1211 result = inv_send_accel(elements, accuracy);
1212 if (result) {
1213 LOG_RESULT_LOCATION(result);
1214 return result;
1215 }
1216
1217 return inv_set_footer();
1218 }
1219
1220 /** Sends linear world accelerometer data to the FIFO. Linear world
1221 * accelerometer data is a 3 length vector of 32-bits. It is the acceleration
1222 * in the world frame with gravity removed. Should be called once after
1223 * inv_dmp_open() and before inv_dmp_start().
1224 *
1225 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of
1226 * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
1227 * for a subset.
1228 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
1229 * bit data.
1230 */
inv_send_linear_accel_in_world(uint_fast16_t elements,uint_fast16_t accuracy)1231 inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements,
1232 uint_fast16_t accuracy)
1233 {
1234 INVENSENSE_FUNC_START;
1235 inv_error_t result;
1236
1237 result = inv_send_linear_accel(INV_ALL, accuracy);
1238 if (result) {
1239 LOG_RESULT_LOCATION(result);
1240 return result;
1241 }
1242 result = inv_send_quaternion(accuracy);
1243
1244 return inv_set_footer();
1245 }
1246
1247 /** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector
1248 * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
1249 * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
1250 * bit data.
1251 */
inv_send_quaternion(uint_fast16_t accuracy)1252 inv_error_t inv_send_quaternion(uint_fast16_t accuracy)
1253 {
1254 INVENSENSE_FUNC_START;
1255 unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
1256 DINA30, DINA38
1257 };
1258 uint_fast16_t elements, kk;
1259 inv_error_t result;
1260
1261 if (inv_get_state() < INV_STATE_DMP_OPENED)
1262 return INV_ERROR_SM_IMPROPER_STATE;
1263
1264 elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4);
1265 accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT);
1266
1267 if (accuracy & INV_16_BIT) {
1268 regs[0] = DINAF8 + 2;
1269 }
1270
1271 fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy;
1272
1273 for (kk = 0; kk < 4; ++kk) {
1274 if ((elements & 1) == 0)
1275 regs[kk + 1] = DINAA0 + 3;
1276 elements >>= 1;
1277 }
1278
1279 result = inv_set_mpu_memory(KEY_CFG_8, 5, regs);
1280 if (result) {
1281 LOG_RESULT_LOCATION(result);
1282 return result;
1283 }
1284
1285 return inv_set_footer();
1286 }
1287
1288 /** Sends raw data to the FIFO.
1289 * Should be called once after inv_dmp_open() and before inv_dmp_start().
1290 * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them
1291 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together
1292 * for a subset. The first element is temperature, the next 3 are gyro data,
1293 * and the last 3 accel data.
1294 * @param accuracy
1295 * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off.
1296 * @return 0 if successful, a non-zero error code otherwise.
1297 */
inv_send_sensor_data(uint_fast16_t elements,uint_fast16_t accuracy)1298 inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy)
1299 {
1300 int result;
1301 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
1302 defined CONFIG_MPU_SENSORS_MPU6050B1
1303 unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1304 DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
1305 DINAA0 + 3
1306 };
1307
1308 if (inv_get_state() < INV_STATE_DMP_OPENED)
1309 return INV_ERROR_SM_IMPROPER_STATE;
1310
1311 if (accuracy)
1312 accuracy = INV_16_BIT;
1313
1314 elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
1315
1316 if (elements & 1)
1317 fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
1318 else
1319 fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
1320 if (elements & 0x7e)
1321 fifo_obj.data_config[CONFIG_RAW_DATA] =
1322 (0x3f & (elements >> 1)) | INV_16_BIT;
1323 else
1324 fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
1325
1326 if (elements & INV_ELEMENT_1) {
1327 regs[0] = DINACA;
1328 }
1329 if (elements & INV_ELEMENT_2) {
1330 regs[1] = DINBC4;
1331 }
1332 if (elements & INV_ELEMENT_3) {
1333 regs[2] = DINACC;
1334 }
1335 if (elements & INV_ELEMENT_4) {
1336 regs[3] = DINBC6;
1337 }
1338 if (elements & INV_ELEMENT_5) {
1339 regs[4] = DINBC0;
1340 }
1341 if (elements & INV_ELEMENT_6) {
1342 regs[5] = DINAC8;
1343 }
1344 if (elements & INV_ELEMENT_7) {
1345 regs[6] = DINBC2;
1346 }
1347 result = inv_set_mpu_memory(KEY_CFG_15, 7, regs);
1348 if (result) {
1349 LOG_RESULT_LOCATION(result);
1350 return result;
1351 }
1352
1353 return inv_set_footer();
1354
1355 #else
1356 INVENSENSE_FUNC_START;
1357 unsigned char regs[4] = { DINAA0 + 3,
1358 DINAA0 + 3,
1359 DINAA0 + 3,
1360 DINAA0 + 3
1361 };
1362
1363 if (inv_get_state() < INV_STATE_DMP_OPENED)
1364 return INV_ERROR_SM_IMPROPER_STATE;
1365
1366 if (accuracy)
1367 accuracy = INV_16_BIT;
1368
1369 elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
1370
1371 if (elements & 0x03) {
1372 elements |= 0x03;
1373 regs[0] = DINA20;
1374 }
1375 if (elements & 0x0C) {
1376 elements |= 0x0C;
1377 regs[1] = DINA28;
1378 }
1379 if (elements & 0x30) {
1380 elements |= 0x30;
1381 regs[2] = DINA30;
1382 }
1383 if (elements & 0x40) {
1384 elements |= 0xC0;
1385 regs[3] = DINA38;
1386 }
1387
1388 result = inv_set_mpu_memory(KEY_CFG_15, 4, regs);
1389 if (result) {
1390 LOG_RESULT_LOCATION(result);
1391 return result;
1392 }
1393
1394 if (elements & 0x01)
1395 fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
1396 else
1397 fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
1398 if (elements & 0xfe)
1399 fifo_obj.data_config[CONFIG_RAW_DATA] =
1400 (0x7f & (elements >> 1)) | INV_16_BIT;
1401 else
1402 fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
1403
1404 return inv_set_footer();
1405 #endif
1406 }
1407
1408 /** Sends raw external data to the FIFO.
1409 * Should be called once after inv_dmp_open() and before inv_dmp_start().
1410 * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
1411 * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
1412 * for a subset.
1413 * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data.
1414 * Sending and Stop sending are reference counted, so data actually
1415 * stops when the reference reaches zero.
1416 */
inv_send_external_sensor_data(uint_fast16_t elements,uint_fast16_t accuracy)1417 inv_error_t inv_send_external_sensor_data(uint_fast16_t elements,
1418 uint_fast16_t accuracy)
1419 {
1420 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
1421 defined CONFIG_MPU_SENSORS_MPU6050B1
1422 int result;
1423 unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3,
1424 DINAA0 + 3, DINAA0 + 3,
1425 DINAA0 + 3, DINAA0 + 3 };
1426
1427 if (inv_get_state() < INV_STATE_DMP_OPENED)
1428 return INV_ERROR_SM_IMPROPER_STATE;
1429
1430 if (accuracy)
1431 accuracy = INV_16_BIT;
1432
1433 elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6);
1434
1435 if (elements)
1436 fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT;
1437 else
1438 fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0;
1439
1440 if (elements & INV_ELEMENT_1) {
1441 regs[0] = DINBC2;
1442 }
1443 if (elements & INV_ELEMENT_2) {
1444 regs[1] = DINACA;
1445 }
1446 if (elements & INV_ELEMENT_3) {
1447 regs[2] = DINBC4;
1448 }
1449 if (elements & INV_ELEMENT_4) {
1450 regs[3] = DINBC0;
1451 }
1452 if (elements & INV_ELEMENT_5) {
1453 regs[4] = DINAC8;
1454 }
1455 if (elements & INV_ELEMENT_6) {
1456 regs[5] = DINACC;
1457 }
1458
1459 result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs);
1460 if (result) {
1461 LOG_RESULT_LOCATION(result);
1462 return result;
1463 }
1464
1465 return inv_set_footer();
1466
1467 #else
1468 return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported
1469 #endif
1470 }
1471
1472 /**
1473 * @brief Send the Quantized Acceleromter data into the FIFO. The data can be
1474 * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel().
1475 *
1476 * To be useful this should be set to fifo_rate + 1 if less than
1477 * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work.
1478 *
1479 * @param elements
1480 * the components bitmask.
1481 * To send all compoents use INV_ALL.
1482 *
1483 * @param accuracy
1484 * Use INV_32_BIT for 32-bit data or INV_16_BIT for
1485 * 16-bit data.
1486 * Set to zero to remove it from the FIFO.
1487 *
1488 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
1489 */
inv_send_quantized_accel(uint_fast16_t elements,uint_fast16_t accuracy)1490 inv_error_t inv_send_quantized_accel(uint_fast16_t elements,
1491 uint_fast16_t accuracy)
1492 {
1493 INVENSENSE_FUNC_START;
1494 unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
1495 DINA30, DINA38
1496 };
1497 unsigned char regs2[4] = { DINA20, DINA28,
1498 DINA30, DINA38
1499 };
1500 inv_error_t result;
1501 int_fast8_t kk;
1502 int_fast8_t ii;
1503
1504 if (inv_get_state() < INV_STATE_DMP_OPENED)
1505 return INV_ERROR_SM_IMPROPER_STATE;
1506
1507 elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8);
1508
1509 if (elements) {
1510 fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT;
1511 } else {
1512 fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0;
1513 }
1514
1515 for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) {
1516 fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0;
1517 for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
1518 fifo_obj.decoded_accel[kk][ii] = 0;
1519 }
1520 }
1521
1522 for (kk = 0; kk < 4; ++kk) {
1523 if ((elements & 1) == 0)
1524 regs[kk + 1] = DINAA0 + 3;
1525 elements >>= 1;
1526 }
1527
1528 result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs);
1529 if (result) {
1530 LOG_RESULT_LOCATION(result);
1531 return result;
1532 }
1533
1534 for (kk = 0; kk < 4; ++kk) {
1535 if ((elements & 1) == 0)
1536 regs2[kk] = DINAA0 + 3;
1537 elements >>= 1;
1538 }
1539
1540 result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2);
1541 if (result) {
1542 LOG_RESULT_LOCATION(result);
1543 return result;
1544 }
1545
1546 return inv_set_footer();
1547 }
1548
inv_send_eis(uint_fast16_t elements,uint_fast16_t accuracy)1549 inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy)
1550 {
1551 INVENSENSE_FUNC_START;
1552 int_fast8_t kk;
1553 unsigned char regs[3] = { DINA28, DINA30, DINA38 };
1554 inv_error_t result;
1555
1556 if (inv_get_state() < INV_STATE_DMP_OPENED)
1557 return INV_ERROR_SM_IMPROPER_STATE;
1558
1559 if (accuracy) {
1560 accuracy = INV_32_BIT;
1561 }
1562
1563 elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3);
1564 accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS);
1565
1566 fifo_obj.data_config[CONFIG_EIS] = elements | accuracy;
1567
1568 for (kk = 0; kk < 3; ++kk) {
1569 if ((elements & 1) == 0)
1570 regs[kk] = DINAA0 + 7;
1571 elements >>= 1;
1572 }
1573
1574 result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs);
1575
1576 return inv_set_footer();
1577 }
1578
1579 /**
1580 * @brief Returns 3-element vector of accelerometer data in body frame.
1581 *
1582 * @param[out] data 3-element vector of accelerometer data in body frame.
1583 * One gee = 2^16.
1584 * @return 0 on success or an error code.
1585 */
inv_get_accel(long * data)1586 inv_error_t inv_get_accel(long *data)
1587 {
1588 int kk;
1589 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
1590
1591 if (data == NULL)
1592 return INV_ERROR_INVALID_PARAMETER;
1593
1594 if ((!fifo_obj.data_config[CONFIG_ACCEL] &&
1595 (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR))
1596 ||
1597 (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
1598 !inv_accel_present()))
1599 return INV_ERROR_FEATURE_NOT_ENABLED;
1600
1601 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
1602 data[kk] = fifo_obj.decoded[REF_ACCEL + kk];
1603 }
1604
1605 return INV_SUCCESS;
1606 }
1607
1608 /**
1609 * @brief Returns 4-element quaternion vector derived from 6-axis or
1610 * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is
1611 * gyros, accel and compass.
1612 *
1613 * @param[out] data 4-element quaternion vector. One is scaled to 2^30.
1614 * @return 0 on success or an error code.
1615 */
inv_get_quaternion(long * data)1616 inv_error_t inv_get_quaternion(long *data)
1617 {
1618 int kk;
1619
1620 if (data == NULL)
1621 return INV_ERROR_INVALID_PARAMETER;
1622
1623 if (!fifo_obj.data_config[CONFIG_QUAT])
1624 return INV_ERROR_FEATURE_NOT_ENABLED;
1625
1626 for (kk = 0; kk < 4; ++kk) {
1627 data[kk] = fifo_obj.decoded[REF_QUATERNION + kk];
1628 }
1629
1630 return INV_SUCCESS;
1631 }
1632
1633 /**
1634 * @brief Returns 4-element quaternion vector derived from 6
1635 * axis sensors (gyros and accels).
1636 * @param[out] data
1637 * 4-element quaternion vector. One is scaled to 2^30.
1638 * @return 0 on success or an error code.
1639 */
inv_get_6axis_quaternion(long * data)1640 inv_error_t inv_get_6axis_quaternion(long *data)
1641 {
1642 int kk;
1643 if (data == NULL)
1644 return INV_ERROR_INVALID_PARAMETER;
1645
1646 if (!fifo_obj.data_config[CONFIG_QUAT])
1647 return INV_ERROR_FEATURE_NOT_ENABLED;
1648
1649 for (kk = 0; kk < 4; ++kk) {
1650 data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk];
1651 }
1652
1653 return INV_SUCCESS;
1654 }
1655
inv_get_relative_quaternion(long * data)1656 inv_error_t inv_get_relative_quaternion(long *data)
1657 {
1658 if (data == NULL)
1659 return INV_ERROR;
1660 data[0] = inv_obj.relative_quat[0];
1661 data[1] = inv_obj.relative_quat[1];
1662 data[2] = inv_obj.relative_quat[2];
1663 data[3] = inv_obj.relative_quat[3];
1664 return INV_SUCCESS;
1665 }
1666
1667 /**
1668 * @brief Returns 3-element vector of gyro data in body frame.
1669 * @param[out] data
1670 * 3-element vector of gyro data in body frame
1671 * with gravity removed. One degree per second = 2^16.
1672 * @return 0 on success or an error code.
1673 */
inv_get_gyro(long * data)1674 inv_error_t inv_get_gyro(long *data)
1675 {
1676 int kk;
1677 if (data == NULL)
1678 return INV_ERROR_INVALID_PARAMETER;
1679
1680 if (fifo_obj.data_config[CONFIG_GYROS]) {
1681 for (kk = 0; kk < 3; ++kk) {
1682 data[kk] = fifo_obj.decoded[REF_GYROS + kk];
1683 }
1684 return INV_SUCCESS;
1685 } else {
1686 return INV_ERROR_FEATURE_NOT_ENABLED;
1687 }
1688 }
1689
1690 /**
1691 * @brief Get the 3-element gravity vector from the FIFO expressed
1692 * in coordinates relative to the body frame.
1693 * @param data
1694 * 3-element vector of gravity in body frame.
1695 * @return 0 on success or an error code.
1696 */
inv_get_gravity(long * data)1697 inv_error_t inv_get_gravity(long *data)
1698 {
1699 long quat[4];
1700 int ii;
1701 inv_error_t result;
1702
1703 if (data == NULL)
1704 return INV_ERROR_INVALID_PARAMETER;
1705
1706 if (!fifo_obj.data_config[CONFIG_QUAT])
1707 return INV_ERROR_FEATURE_NOT_ENABLED;
1708
1709 if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) {
1710 fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY;
1711
1712 // Compute it from Quaternion
1713 result = inv_get_quaternion(quat);
1714 if (result) {
1715 LOG_RESULT_LOCATION(result);
1716 return result;
1717 }
1718
1719 data[0] =
1720 inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
1721 data[1] =
1722 inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
1723 data[2] =
1724 (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) -
1725 1073741824L;
1726
1727 for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
1728 data[ii] >>= 14;
1729 fifo_obj.gravity_cache[ii] = data[ii];
1730 }
1731 } else {
1732 data[0] = fifo_obj.gravity_cache[0];
1733 data[1] = fifo_obj.gravity_cache[1];
1734 data[2] = fifo_obj.gravity_cache[2];
1735 }
1736
1737 return INV_SUCCESS;
1738 }
1739
1740 /**
1741 * @brief Sets the filter coefficent used for computing the acceleration
1742 * bias which is used to compute linear acceleration.
1743 * @param[in] coef Fitler coefficient. 0. means no filter, a small number means
1744 * a small cutoff frequency with an increasing number meaning
1745 * an increasing cutoff frequency.
1746 */
inv_set_linear_accel_filter_coef(float coef)1747 inv_error_t inv_set_linear_accel_filter_coef(float coef)
1748 {
1749 fifo_obj.acc_filter_coef = coef;
1750 return INV_SUCCESS;
1751 }
1752
1753 /**
1754 * @brief Returns 3-element vector of accelerometer data in body frame
1755 * with gravity removed.
1756 * @param[out] data 3-element vector of accelerometer data in body frame
1757 * with gravity removed. One g = 2^16.
1758 * @return 0 on success or an error code. data unchanged on error.
1759 */
inv_get_linear_accel(long * data)1760 inv_error_t inv_get_linear_accel(long *data)
1761 {
1762 int kk;
1763 long grav[3];
1764 long la[3];
1765 inv_error_t result;
1766
1767 if (data == NULL)
1768 return INV_ERROR_INVALID_PARAMETER;
1769
1770 result = inv_get_gravity(grav);
1771 if (result) {
1772 LOG_RESULT_LOCATION(result);
1773 return result;
1774 }
1775 result = inv_get_accel(la);
1776 if (result) {
1777 LOG_RESULT_LOCATION(result);
1778 return result;
1779 }
1780
1781 if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) {
1782 fifo_obj.cache |= FIFO_CACHE_ACC_BIAS;
1783
1784 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
1785 long x;
1786 x = la[kk] - grav[kk];
1787 fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef +
1788 fifo_obj.acc_bias_filt[kk] *
1789 (1.f -
1790 fifo_obj.acc_filter_coef));
1791 data[kk] = x - fifo_obj.acc_bias_filt[kk];
1792 }
1793 } else {
1794 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
1795 data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk];
1796 }
1797 }
1798 return INV_SUCCESS;
1799 }
1800
1801 /**
1802 * @brief Returns 3-element vector of accelerometer data in world frame
1803 * with gravity removed.
1804 * @param[out] data 3-element vector of accelerometer data in world frame
1805 * with gravity removed. One g = 2^16.
1806 * @return 0 on success or an error code.
1807 */
inv_get_linear_accel_in_world(long * data)1808 inv_error_t inv_get_linear_accel_in_world(long *data)
1809 {
1810 int kk;
1811 if (data == NULL)
1812 return INV_ERROR_INVALID_PARAMETER;
1813 if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) {
1814 long wtemp[4], qi[4], wtemp2[4];
1815 wtemp[0] = 0;
1816 inv_get_linear_accel(&wtemp[1]);
1817 inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2);
1818 inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi);
1819 inv_q_mult(wtemp2, qi, wtemp);
1820 for (kk = 0; kk < 3; ++kk) {
1821 data[kk] = wtemp[kk + 1];
1822 }
1823 return INV_SUCCESS;
1824 } else {
1825 return INV_ERROR_FEATURE_NOT_ENABLED;
1826 }
1827 }
1828
1829 /**
1830 * @brief Returns 4-element vector of control data.
1831 * @param[out] data 4-element vector of control data.
1832 * @return 0 for succes or an error code.
1833 */
inv_get_cntrl_data(long * data)1834 inv_error_t inv_get_cntrl_data(long *data)
1835 {
1836 int kk;
1837 if (data == NULL)
1838 return INV_ERROR_INVALID_PARAMETER;
1839
1840 if (!fifo_obj.data_config[CONFIG_CONTROL_DATA])
1841 return INV_ERROR_FEATURE_NOT_ENABLED;
1842
1843 for (kk = 0; kk < 4; ++kk) {
1844 data[kk] = fifo_obj.decoded[REF_CONTROL + kk];
1845 }
1846
1847 return INV_SUCCESS;
1848
1849 }
1850
1851 /**
1852 * @brief Returns 3-element vector of EIS shfit data
1853 * @param[out] data 3-element vector of EIS shift data.
1854 * @return 0 for succes or an error code.
1855 */
inv_get_eis(long * data)1856 inv_error_t inv_get_eis(long *data)
1857 {
1858 int kk;
1859 if (data == NULL)
1860 return INV_ERROR_INVALID_PARAMETER;
1861
1862 if (!fifo_obj.data_config[CONFIG_EIS])
1863 return INV_ERROR_FEATURE_NOT_ENABLED;
1864
1865 for (kk = 0; kk < 3; ++kk) {
1866 data[kk] = fifo_obj.decoded[REF_EIS + kk];
1867 }
1868
1869 return INV_SUCCESS;
1870
1871 }
1872
1873 /**
1874 * @brief Returns 3-element vector of accelerometer data in body frame.
1875 * @param[out] data 3-element vector of accelerometer data in body frame in g's.
1876 * @return 0 for success or an error code.
1877 */
inv_get_accel_float(float * data)1878 inv_error_t inv_get_accel_float(float *data)
1879 {
1880 long lData[3];
1881 int kk;
1882 int result;
1883
1884 if (data == NULL)
1885 return INV_ERROR_INVALID_PARAMETER;
1886
1887 result = inv_get_accel(lData);
1888 if (result) {
1889 LOG_RESULT_LOCATION(result);
1890 return result;
1891 }
1892
1893 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
1894 data[kk] = lData[kk] / 65536.0f;
1895 }
1896
1897 return INV_SUCCESS;
1898 }
1899
1900 /**
1901 * @brief Returns 4-element quaternion vector.
1902 * @param[out] data 4-element quaternion vector.
1903 * @return 0 on success, an error code otherwise.
1904 */
inv_get_quaternion_float(float * data)1905 inv_error_t inv_get_quaternion_float(float *data)
1906 {
1907 int kk;
1908
1909 if (data == NULL)
1910 return INV_ERROR_INVALID_PARAMETER;
1911
1912 if (!fifo_obj.data_config[CONFIG_QUAT])
1913 return INV_ERROR_FEATURE_NOT_ENABLED;
1914
1915 for (kk = 0; kk < 4; ++kk) {
1916 data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f;
1917 }
1918
1919 return INV_SUCCESS;
1920 }
1921
1922 /**
1923 * @brief Command the MPU to put data in the FIFO at a particular rate.
1924 *
1925 * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For
1926 * example if the MPU is running at 200Hz the following values apply:
1927 *
1928 * <TABLE>
1929 * <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR>
1930 * <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR>
1931 * <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR>
1932 * <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR>
1933 * <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR>
1934 * <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR>
1935 * <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR>
1936 * </TABLE>
1937 *
1938 * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED)
1939 * then inv_run_state_callbacks() will be called to allow features
1940 * that depend upon fundamental constants to be updated.
1941 *
1942 * @pre inv_dmp_open()
1943 * @ifnot MPL_MF
1944 * or inv_open_low_power_pedometer()
1945 * or inv_eis_open_dmp()
1946 * @endif
1947 * and inv_dmp_start()
1948 * must <b>NOT</b> have been called.
1949 *
1950 * @param fifoRate Divider value - 1. Output rate is
1951 * (DMP Sample Rate) / (fifoRate + 1).
1952 *
1953 * @return INV_SUCCESS if successful, ML error code on any failure.
1954 */
inv_set_fifo_rate(unsigned short fifoRate)1955 inv_error_t inv_set_fifo_rate(unsigned short fifoRate)
1956 {
1957 INVENSENSE_FUNC_START;
1958 unsigned char regs[2];
1959 unsigned char state;
1960 inv_error_t result = INV_SUCCESS;
1961 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
1962
1963 state = inv_get_state();
1964 if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED)
1965 return INV_ERROR_SM_IMPROPER_STATE;
1966
1967 fifo_obj.fifo_rate = fifoRate;
1968
1969 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
1970
1971 regs[0] = (unsigned char)((fifoRate >> 8) & 0xff);
1972 regs[1] = (unsigned char)(fifoRate & 0xff);
1973
1974 result = inv_set_mpu_memory(KEY_D_0_22, 2, regs);
1975 if (result) {
1976 LOG_RESULT_LOCATION(result);
1977 return result;
1978 }
1979 fifo_obj.sample_step_size_ms =
1980 (unsigned short)(((long)fifoRate + 1) *
1981 (inv_mpu_get_sampling_period_us
1982 (mldl_cfg)) / 1000L);
1983
1984 if (state == INV_STATE_DMP_STARTED)
1985 result = inv_run_state_callbacks(state);
1986 } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) {
1987 struct ext_slave_config config;
1988 long data;
1989 config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
1990 config.len = sizeof(long);
1991 config.apply = (state == INV_STATE_DMP_STARTED);
1992 config.data = &data;
1993 data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1);
1994
1995 /* Ask for the same frequency */
1996 result = inv_mpu_config_accel(mldl_cfg,
1997 inv_get_serial_handle(),
1998 inv_get_serial_handle(), &config);
1999 if (result) {
2000 LOG_RESULT_LOCATION(result);
2001 return result;
2002 }
2003 result = inv_mpu_get_accel_config(mldl_cfg,
2004 inv_get_serial_handle(),
2005 inv_get_serial_handle(), &config);
2006 if (result) {
2007 LOG_RESULT_LOCATION(result);
2008 return result;
2009 }
2010 if(FIFO_DEBUG)
2011 MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000);
2012 /* Record the actual frequency granted odr is in mHz */
2013 fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data);
2014 }
2015 return result;
2016 }
2017
2018 /**
2019 * @brief Retrieve the current FIFO update divider - 1.
2020 * See inv_set_fifo_rate() for how this value is used.
2021 *
2022 * The fifo rate when there is no fifo is the equivilent divider when
2023 * derived from the value set by SetSampleSteSizeMs()
2024 *
2025 * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error.
2026 */
inv_get_fifo_rate(void)2027 unsigned short inv_get_fifo_rate(void)
2028 {
2029 return fifo_obj.fifo_rate;
2030 }
2031
2032 /**
2033 * @brief Returns the step size for quaternion type data.
2034 *
2035 * Typically the data rate for each FIFO packet. When the gryos are sleeping
2036 * this value will return the last value set by SetSampleStepSizeMs()
2037 *
2038 * @return step size for quaternion type data
2039 */
inv_get_sample_step_size_ms(void)2040 int_fast16_t inv_get_sample_step_size_ms(void)
2041 {
2042 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
2043
2044 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
2045 return (fifo_obj.fifo_rate + 1) *
2046 (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000);
2047 else
2048 return fifo_obj.sample_step_size_ms;
2049 }
2050
2051 /**
2052 * @brief Returns the step size for quaternion type data.
2053 *
2054 * Typically the data rate for each FIFO packet. When the gryos are sleeping
2055 * this value will return the last value set by SetSampleStepSizeMs()
2056 *
2057 * @return step size for quaternion type data
2058 */
inv_get_sample_frequency(void)2059 int_fast16_t inv_get_sample_frequency(void)
2060 {
2061 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
2062
2063 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
2064 return (inv_mpu_get_sampling_rate_hz(mldl_cfg) /
2065 (fifo_obj.fifo_rate + 1));
2066 else
2067 return (1000 / fifo_obj.sample_step_size_ms);
2068 }
2069
2070 /**
2071 * @brief The gyro data magnitude squared :
2072 * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
2073 * @return the computed magnitude squared output of the gyroscope.
2074 */
inv_get_gyro_sum_of_sqr(void)2075 unsigned long inv_get_gyro_sum_of_sqr(void)
2076 {
2077 unsigned long gmag = 0;
2078 long temp;
2079 int kk;
2080
2081 for (kk = 0; kk < 3; ++kk) {
2082 temp = fifo_obj.decoded[REF_GYROS + kk] >>
2083 (16 - (GYRO_MAG_SQR_SHIFT / 2));
2084 gmag += temp * temp;
2085 }
2086
2087 return gmag;
2088 }
2089
2090 /**
2091 * @brief The gyro data magnitude squared:
2092 * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT.
2093 * @return the computed magnitude squared output of the accelerometer.
2094 */
inv_accel_sum_of_sqr(void)2095 unsigned long inv_accel_sum_of_sqr(void)
2096 {
2097 unsigned long amag = 0;
2098 long temp;
2099 int kk;
2100 long accel[3];
2101 inv_error_t result;
2102
2103 result = inv_get_accel(accel);
2104 if (INV_SUCCESS != result) {
2105 return 0;
2106 }
2107
2108 for (kk = 0; kk < 3; ++kk) {
2109 temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2));
2110 amag += temp * temp;
2111 }
2112 return amag;
2113 }
2114
2115 /**
2116 * @internal
2117 */
inv_override_quaternion(float * q)2118 void inv_override_quaternion(float *q)
2119 {
2120 int kk;
2121 for (kk = 0; kk < 4; ++kk) {
2122 fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30));
2123 }
2124 }
2125
2126 /**
2127 * @internal
2128 * @brief This registers a function to be called for each set of
2129 * gyro/quaternion/rotation matrix/etc output.
2130 * @param[in] func The callback function to register
2131 * @param[in] priority The unique priority number of the callback. Lower numbers
2132 * are called first.
2133 * @return error code.
2134 */
inv_register_fifo_rate_process(inv_obj_func func,int priority)2135 inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority)
2136 {
2137 INVENSENSE_FUNC_START;
2138 inv_error_t result;
2139 int kk, nn;
2140
2141 result = inv_lock_mutex(fifo_rate_obj.mutex);
2142 if (INV_SUCCESS != result) {
2143 return result;
2144 }
2145 // Make sure we haven't registered this function already
2146 // Or used the same priority
2147 for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
2148 if ((fifo_rate_obj.fifo_process_cb[kk] == func) ||
2149 (fifo_rate_obj.priority[kk] == priority)) {
2150 inv_unlock_mutex(fifo_rate_obj.mutex);
2151 return INV_ERROR_INVALID_PARAMETER;
2152 }
2153 }
2154
2155 // Make sure we have not filled up our number of allowable callbacks
2156 if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) {
2157 kk = 0;
2158 if (fifo_rate_obj.num_cb != 0) {
2159 // set kk to be where this new callback goes in the array
2160 while ((kk < fifo_rate_obj.num_cb) &&
2161 (fifo_rate_obj.priority[kk] < priority)) {
2162 kk++;
2163 }
2164 if (kk != fifo_rate_obj.num_cb) {
2165 // We need to move the others
2166 for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) {
2167 fifo_rate_obj.fifo_process_cb[nn] =
2168 fifo_rate_obj.fifo_process_cb[nn - 1];
2169 fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1];
2170 }
2171 }
2172 }
2173 // Add new callback
2174 fifo_rate_obj.fifo_process_cb[kk] = func;
2175 fifo_rate_obj.priority[kk] = priority;
2176 fifo_rate_obj.num_cb++;
2177 } else {
2178 result = INV_ERROR_MEMORY_EXAUSTED;
2179 }
2180
2181 inv_unlock_mutex(fifo_rate_obj.mutex);
2182 return result;
2183 }
2184
2185 /**
2186 * @internal
2187 * @brief This unregisters a function to be called for each set of
2188 * gyro/quaternion/rotation matrix/etc output.
2189 * @return error code.
2190 */
inv_unregister_fifo_rate_process(inv_obj_func func)2191 inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func)
2192 {
2193 INVENSENSE_FUNC_START;
2194 int kk, jj;
2195 inv_error_t result;
2196
2197 result = inv_lock_mutex(fifo_rate_obj.mutex);
2198 if (INV_SUCCESS != result) {
2199 return result;
2200 }
2201 // Make sure we haven't registered this function already
2202 result = INV_ERROR_INVALID_PARAMETER;
2203 for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
2204 if (fifo_rate_obj.fifo_process_cb[kk] == func) {
2205 for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) {
2206 fifo_rate_obj.fifo_process_cb[jj - 1] =
2207 fifo_rate_obj.fifo_process_cb[jj];
2208 fifo_rate_obj.priority[jj - 1] =
2209 fifo_rate_obj.priority[jj];
2210 }
2211 fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL;
2212 fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0;
2213 fifo_rate_obj.num_cb--;
2214 result = INV_SUCCESS;
2215 break;
2216 }
2217 }
2218
2219 inv_unlock_mutex(fifo_rate_obj.mutex);
2220 return result;
2221
2222 }
2223 #ifdef UMPL
2224 bool bFIFIDataAvailable = FALSE;
isUmplDataInFIFO(void)2225 bool isUmplDataInFIFO(void)
2226 {
2227 return bFIFIDataAvailable;
2228 }
setUmplDataInFIFOFlag(bool flag)2229 void setUmplDataInFIFOFlag(bool flag)
2230 {
2231 bFIFIDataAvailable = flag;
2232 }
2233 #endif
inv_run_fifo_rate_processes(void)2234 inv_error_t inv_run_fifo_rate_processes(void)
2235 {
2236 int kk;
2237 inv_error_t result, result2;
2238
2239 result = inv_lock_mutex(fifo_rate_obj.mutex);
2240 if (INV_SUCCESS != result) {
2241 MPL_LOGE("MLOsLockMutex returned %d\n", result);
2242 return result;
2243 }
2244 // User callbacks take priority over the fifo_process_cb callback
2245 if (fifo_obj.fifo_process_cb)
2246 fifo_obj.fifo_process_cb();
2247
2248 for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
2249 if (fifo_rate_obj.fifo_process_cb[kk]) {
2250 result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj);
2251 if (result == INV_SUCCESS)
2252 #ifdef UMPL
2253 setUmplDataInFIFOFlag(TRUE);
2254 #endif
2255 result = result2;
2256 }
2257 }
2258
2259 inv_unlock_mutex(fifo_rate_obj.mutex);
2260 return result;
2261 }
2262
2263 /*********************/
2264 /** \}*//* defgroup */
2265 /*********************/
2266