1 /**
2 * Gesture Test application for Invensense's MPU6/9xxx (w/ DMP).
3 */
4
5 #include <unistd.h>
6 #include <dirent.h>
7 #include <fcntl.h>
8 #include <stdio.h>
9 #include <errno.h>
10 #include <sys/stat.h>
11 #include <stdlib.h>
12 #include <features.h>
13 #include <dirent.h>
14 #include <string.h>
15 #include <poll.h>
16 #include <stddef.h>
17 #include <linux/input.h>
18 #include <time.h>
19 #include <linux/time.h>
20 #include <unistd.h>
21 #include <termios.h>
22
23 #include "invensense.h"
24 #include "ml_math_func.h"
25 #include "storage_manager.h"
26 #include "ml_stored_data.h"
27 #include "ml_sysfs_helper.h"
28 #include "mlos.h"
29
30 //#define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */
31
32 #define SUPPORT_SCREEN_ORIENTATION
33 //#define SUPPORT_TAP
34 //#define SUPPORT_ORIENTATION
35 #define SUPPORT_PEDOMETER
36 #define SUPPORT_SMD
37
38 #define MAX_SYSFS_NAME_LEN (100)
39 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
40 #define IIO_SYSFS_PATH "/sys/bus/iio/devices/iio:device0"
41 #define IIO_HUB_NAME "inv_hub"
42
43 #define POLL_TIME (2000) // 2sec
44
45 struct sysfs_attrbs {
46 char *name;
47 char *enable;
48 char *power_state;
49 char *dmp_on;
50 char *dmp_int_on;
51 char *dmp_firmware;
52 char *firmware_loaded;
53 #ifdef SUPPORT_SCREEN_ORIENTATION
54 char *event_display_orientation;
55 char *display_orientation_on;
56 #endif
57 #ifdef SUPPORT_ORIENTATION
58 char *event_orientation;
59 char *orientation_on;
60 #endif
61 #ifdef SUPPORT_TAP
62 char *event_tap;
63 char *tap_min_count;
64 char *tap_on;
65 char *tap_threshold;
66 char *tap_time;
67 #endif
68 #ifdef SUPPORT_PEDOMETER
69 char *pedometer_on;
70 char *pedometer_steps;
71 char *pedometer_time;
72 #endif
73 #ifdef SUPPORT_SMD
74 char *event_smd;
75 char *smd_enable;
76 char *smd_threshold;
77 char *smd_delay_threshold;
78 char *smd_delay_threshold2;
79 #endif
80 } mpu;
81
82 enum {
83 #ifdef SUPPORT_TAP
84 FEAT_TAP,
85 #endif
86 #ifdef SUPPORT_SCREEN_ORIENTATION
87 FEAT_SCREEN_ORIENTATION,
88 #endif
89 #ifdef SUPPORT_ORIENTATION
90 FEAT_ORIENTATION,
91 #endif
92 #ifdef SUPPORT_PEDOMETER
93 FEAT_PEDOMETER,
94 #endif
95 #ifdef SUPPORT_SMD
96 FEAT_SMD,
97 #endif
98
99 NUM_DMP_FEATS
100 };
101
102 char *sysfs_names_ptr;
103 #ifdef SUPPORT_PEDOMETER
104 unsigned long last_pedometer_poll = 0L;
105 unsigned long pedometer_poll_timeout = 500L; // .5 second
106 #endif
107 struct pollfd pfd[NUM_DMP_FEATS];
108 bool android_hub = false; // flag to indicate true=Hub, false=non-hub
109
110 /*******************************************************************************
111 * DMP Feature Supported Functions
112 ******************************************************************************/
113
read_sysfs_int(char * filename,int * var)114 int read_sysfs_int(char *filename, int *var)
115 {
116 int res=0;
117 FILE *fp;
118
119 fp = fopen(filename, "r");
120 if (fp!=NULL) {
121 fscanf(fp, "%d\n", var);
122 fclose(fp);
123 } else {
124 printf("ERR open file to read: %s\n", filename);
125 res= -1;
126 }
127 return res;
128 }
129
write_sysfs_int(char * filename,int data)130 int write_sysfs_int(char *filename, int data)
131 {
132 int res=0;
133 FILE *fp;
134
135 #ifdef DEBUG_PRINT
136 printf("writing '%s' with '%d'\n", filename, data);
137 #endif
138
139 fp = fopen(filename, "w");
140 if (fp != NULL) {
141 fprintf(fp, "%d\n", data);
142 fclose(fp);
143 } else {
144 printf("ERR open file to write: %s\n", filename);
145 res = -1;
146 }
147 return res;
148 }
149
150 /**************************************************
151 This _kbhit() function is courtesy of the web
152 ***************************************************/
_kbhit(void)153 int _kbhit(void)
154 {
155 static const int STDIN = 0;
156 static bool initialized = false;
157
158 if (! initialized) {
159 // Use termios to turn off line buffering
160 struct termios term;
161 tcgetattr(STDIN, &term);
162 term.c_lflag &= ~ICANON;
163 tcsetattr(STDIN, TCSANOW, &term);
164 setbuf(stdin, NULL);
165 initialized = true;
166 }
167
168 int bytesWaiting;
169 ioctl(STDIN, FIONREAD, &bytesWaiting);
170 return bytesWaiting;
171 }
172
inv_init_sysfs_attributes(void)173 int inv_init_sysfs_attributes(void)
174 {
175 unsigned char i = 0;
176 char sysfs_path[MAX_SYSFS_NAME_LEN];
177 char *sptr;
178 char **dptr;
179
180 sysfs_names_ptr =
181 (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
182 sptr = sysfs_names_ptr;
183 if (sptr != NULL) {
184 dptr = (char**)&mpu;
185 do {
186 *dptr++ = sptr;
187 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
188 } while (++i < MAX_SYSFS_ATTRB);
189 } else {
190 printf("couldn't alloc mem for sysfs paths\n");
191 return -1;
192 }
193
194 // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
195 inv_get_sysfs_path(sysfs_path);
196
197 sprintf(mpu.name, "%s%s", sysfs_path, "/name");
198 sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
199 sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
200 sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
201 sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
202 sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
203 sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
204
205 #ifdef SUPPORT_SCREEN_ORIENTATION
206 sprintf(mpu.event_display_orientation, "%s%s",
207 sysfs_path, "/event_display_orientation");
208 sprintf(mpu.display_orientation_on, "%s%s",
209 sysfs_path, "/display_orientation_on");
210 #endif
211 #ifdef SUPPORT_ORIENTATION
212 sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
213 sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
214 #endif
215 #ifdef SUPPORT_TAP
216 sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
217 sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
218 sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
219 sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
220 sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
221 #endif
222 #ifdef SUPPORT_PEDOMETER
223 sprintf(mpu.pedometer_on, "%s%s", sysfs_path, "/dmp_on");
224 sprintf(mpu.pedometer_steps, "%s%s", sysfs_path, "/pedometer_steps");
225 sprintf(mpu.pedometer_time, "%s%s", sysfs_path, "/pedometer_time");
226 #endif
227 #ifdef SUPPORT_SMD
228 sprintf(mpu.event_smd, "%s%s", sysfs_path, "/event_smd");
229 sprintf(mpu.smd_enable, "%s%s", sysfs_path, "/smd_enable");
230 sprintf(mpu.smd_threshold, "%s%s", sysfs_path, "/smd_threshold");
231 sprintf(mpu.smd_delay_threshold, "%s%s",
232 sysfs_path, "/smd_delay_threshold");
233 sprintf(mpu.smd_delay_threshold2, "%s%s",
234 sysfs_path, "/smd_delay_threshold2");
235 #endif
236
237 #if 0
238 // test print sysfs paths
239 dptr = (char**)&mpu;
240 for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
241 MPL_LOGE("sysfs path: %s", *dptr++);
242 }
243 #endif
244 return 0;
245 }
246
dmp_fw_loaded(void)247 int dmp_fw_loaded(void)
248 {
249 int fw_loaded;
250 if (read_sysfs_int(mpu.firmware_loaded, &fw_loaded) < 0)
251 fw_loaded= 0;
252 return fw_loaded;
253 }
254
is_android_hub(void)255 int is_android_hub(void)
256 {
257 char dev_name[8];
258 FILE *fp;
259
260 fp= fopen(mpu.name, "r");
261 fgets(dev_name, 8, fp);
262 fclose(fp);
263
264 if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME))) {
265 android_hub = true;
266 }else {
267 android_hub = false;
268 }
269
270 return 0;
271 }
272
273 /*
274 Enablers for the gestures
275 */
276
master_enable(int en)277 int master_enable(int en)
278 {
279 if (write_sysfs_int(mpu.enable, en) < 0) {
280 printf("GT:ERR-can't write 'buffer/enable'");
281 return -1;
282 }
283 return 0;
284 }
285
286 #ifdef SUPPORT_TAP
enable_tap(int en)287 int enable_tap(int en)
288 {
289 if (write_sysfs_int(mpu.tap_on, en) < 0) {
290 printf("GT:ERR-can't write 'tap_on'\n");
291 return -1;
292 }
293
294 return 0;
295 }
296 #endif
297
298 /* Unnecessary: pedometer_on == dmp_on, which is always on
299 #ifdef SUPPORT_PEDOMETER
300 int enable_pedometer(int en)
301 {
302 if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
303 printf("GT:ERR-can't write 'pedometer_on'\n");
304 return -1;
305 }
306
307 return 0;
308 }
309 #endif
310 */
311
312 #ifdef SUPPORT_SCREEN_ORIENTATION
enable_display_orientation(int en)313 int enable_display_orientation(int en)
314 {
315 if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
316 printf("GT:ERR-can't write 'display_orientation_on'\n");
317 return -1;
318 }
319
320 return 0;
321 }
322 #endif
323
324 #ifdef SUPPORT_ORIENTATION
enable_orientation(int en)325 int enable_orientation(int en)
326 {
327 if (write_sysfs_int(mpu.orientation_on, en) < 0) {
328 printf("GT:ERR-can't write 'orientation_on'\n");
329 return -1;
330 }
331
332 return 0;
333 }
334 #endif
335
336 #ifdef SUPPORT_SMD
enable_smd(int en)337 int enable_smd(int en)
338 {
339 if (write_sysfs_int(mpu.smd_enable, en) < 0) {
340 printf("GT:ERR-can't write 'smd_enable'\n");
341 return -1;
342 }
343 return 0;
344 }
345 #endif
346
347 /*
348 Handlers for the gestures
349 */
350 #ifdef SUPPORT_TAP
tap_handler(void)351 int tap_handler(void)
352 {
353 FILE *fp;
354 int tap, tap_dir, tap_num;
355
356 fp = fopen(mpu.event_tap, "rt");
357 fscanf(fp, "%d\n", &tap);
358 fclose(fp);
359
360 tap_dir = tap/8;
361 tap_num = tap%8 + 1;
362
363 #ifdef DEBUG_PRINT
364 printf("GT:Tap Handler **\n");
365 printf("Tap= %x\n", tap);
366 printf("Tap Dir= %x\n", tap_dir);
367 printf("Tap Num= %x\n", tap_num);
368 #endif
369
370 switch (tap_dir) {
371 case 1:
372 printf("Tap Axis->X Pos, ");
373 break;
374 case 2:
375 printf("Tap Axis->X Neg, ");
376 break;
377 case 3:
378 printf("Tap Axis->Y Pos, ");
379 break;
380 case 4:
381 printf("Tap Axis->Y Neg, ");
382 break;
383 case 5:
384 printf("Tap Axis->Z Pos, ");
385 break;
386 case 6:
387 printf("Tap Axis->Z Neg, ");
388 break;
389 default:
390 printf("Tap Axis->Unknown, ");
391 break;
392 }
393 printf("#%d\n", tap_num);
394
395 return 0;
396 }
397 #endif
398
399 #ifdef SUPPORT_PEDOMETER
pedometer_handler(void)400 int pedometer_handler(void)
401 {
402 FILE *fp;
403 static int last_pedometer_steps = -1;
404 static long last_pedometer_time = -1;
405 int pedometer_steps;
406 long pedometer_time;
407
408 #ifdef DEBUG_PRINT
409 printf("GT:Pedometer Handler\n");
410 #endif
411
412 fp = fopen(mpu.pedometer_steps, "rt");
413 fscanf(fp, "%d\n", &pedometer_steps);
414 fclose(fp);
415
416 fp = fopen(mpu.pedometer_time, "rt");
417 fscanf(fp, "%ld\n", &pedometer_time);
418 fclose(fp);
419
420 if (last_pedometer_steps == -1 && last_pedometer_time == -1) {
421 printf("Pedometer Steps: %d Time: %ld ",
422 pedometer_steps, pedometer_time);
423 if (pedometer_steps > 10
424 || pedometer_time > (pedometer_poll_timeout * 2))
425 printf("(resumed)\n");
426 else
427 printf("\n");
428 } else if (last_pedometer_steps != pedometer_steps
429 || last_pedometer_time != pedometer_time) {
430 printf("Pedometer Steps: %d Time: %ld\n",
431 pedometer_steps, pedometer_time);
432 }
433
434 last_pedometer_steps = pedometer_steps;
435 last_pedometer_time = pedometer_time;
436
437 return 0;
438 }
439 #endif
440
441 #ifdef SUPPORT_SCREEN_ORIENTATION
display_orientation_handler(void)442 int display_orientation_handler(void)
443 {
444 FILE *fp;
445 int orient;
446
447 #ifdef DEBUG_PRINT
448 printf("GT:Screen Orient Handler\n");
449 #endif
450
451 fp = fopen(mpu.event_display_orientation, "rt");
452 if (!fp) {
453 printf("GT:Cannot open '%s'\n", mpu.event_display_orientation);
454 return -1;
455 }
456 fscanf(fp, "%d\n", &orient);
457 fclose(fp);
458
459 printf("Screen Orient-> %d\n", orient);
460
461 return 0;
462 }
463 #endif
464
465 #ifdef SUPPORT_ORIENTATION
host_orientation_handler(void)466 int host_orientation_handler(void)
467 {
468 FILE *fp;
469 int orient;
470
471 fp = fopen(mpu.event_orientation, "rt");
472 fscanf(fp, "%d\n", &orient);
473 fclose(fp);
474
475 #ifdef DEBUG_PRINT
476 printf("GT:Reg Orient Handler\n");
477 #endif
478
479 if (orient & 0x01)
480 printf("Orient->X Up\n");
481 if (orient & 0x02)
482 printf("Orient->X Down\n");
483 if (orient & 0x04)
484 printf("Orient->Y Up\n");
485 if (orient & 0x08)
486 printf("Orient->Y Down\n");
487 if (orient & 0x10)
488 printf("Orient->Z Up\n");
489 if (orient & 0x20)
490 printf("Orient->Z Down\n");
491 if (orient & 0x40)
492 printf("Orient->Flip\n");
493
494 return 0;
495 }
496 #endif
497
498 #ifdef SUPPORT_SMD
smd_handler(void)499 int smd_handler(void)
500 {
501 FILE *fp;
502 int smd;
503
504 fp = fopen(mpu.event_smd, "rt");
505 fscanf(fp, "%d\n", &smd);
506 fclose(fp);
507
508 #ifdef DEBUG_PRINT
509 printf("GT:SMD Handler\n");
510 #endif
511 printf("SMD (%d)\n", smd);
512
513 /* wait for the acceleration low pass filtered tail to die off -
514 this is to prevent that the tail end of a 2nd event of above threhsold
515 motion be considered as also the 1st event for the next SM detection */
516 inv_sleep(1000);
517
518 /* re-enable to continue the detection */
519 master_enable(0);
520 enable_smd(1);
521 master_enable(1);
522
523 return 0;
524 }
525 #endif
526
enable_dmp_features(int en)527 int enable_dmp_features(int en)
528 {
529 int res= -1;
530
531 if (android_hub || dmp_fw_loaded()) {
532 /* Currently there's no info regarding DMP's supported features/capabilities
533 An error in enabling features below could be an indication of the feature
534 not supported in current loaded DMP firmware */
535
536 master_enable(0);
537 #ifdef SUPPORT_TAP
538 enable_tap(en);
539 #endif
540 #ifdef SUPPORT_SCREEN_ORIENTATION
541 enable_display_orientation(en);
542 #endif
543 #ifdef SUPPORT_ORIENTATION
544 if (android_hub == false) {
545 // Android Hub does not support 'regular' orientation feature
546 enable_orientation(en);
547 }
548 #endif
549 #ifdef SUPPORT_SMD
550 enable_smd(en);
551 #endif
552 master_enable(1);
553 res = 0;
554
555 } else {
556 printf("GT:ERR-No DMP firmware\n");
557 res= -1;
558 }
559
560 return res;
561 }
562
init_fds(void)563 int init_fds(void)
564 {
565 int i;
566
567 for (i = 0; i < NUM_DMP_FEATS; i++) {
568 switch(i) {
569 #ifdef SUPPORT_TAP
570 case FEAT_TAP:
571 pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
572 break;
573 #endif
574 #ifdef SUPPORT_SCREEN_ORIENTATION
575 case FEAT_SCREEN_ORIENTATION:
576 pfd[i].fd = open(mpu.event_display_orientation,
577 O_RDONLY | O_NONBLOCK);
578 break;
579 #endif
580 #ifdef SUPPORT_ORIENTATION
581 case FEAT_ORIENTATION:
582 pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
583 break;
584 #endif
585 #ifdef SUPPORT_SMD
586 case FEAT_SMD:
587 pfd[i].fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
588 break;
589 #endif
590 default:
591 pfd[i].fd = -1;
592 }
593
594 pfd[i].events = POLLPRI|POLLERR,
595 pfd[i].revents = 0;
596 }
597
598 return 0;
599 }
600
parse_events(struct pollfd pfd[],int num_fds)601 void parse_events(struct pollfd pfd[], int num_fds)
602 {
603 int i;
604
605 for (i = 0; i < num_fds; i++) {
606 if(pfd[i].revents != 0) {
607 switch(i) {
608 #ifdef SUPPORT_TAP
609 case FEAT_TAP:
610 tap_handler();
611 break;
612 #endif
613 #ifdef SUPPORT_SCREEN_ORIENTATION
614 case FEAT_SCREEN_ORIENTATION:
615 display_orientation_handler();
616 break;
617 #endif
618 #ifdef SUPPORT_ORIENTATION
619 case FEAT_ORIENTATION:
620 host_orientation_handler();
621 break;
622 #endif
623 #ifdef SUPPORT_SMD
624 case FEAT_SMD:
625 smd_handler();
626 break;
627 #endif
628 default:
629 printf("GT:ERR-unhandled/unrecognized gesture event");
630 break;
631 }
632 pfd[i].revents = 0; // no need: reset anyway
633 }
634 }
635
636 #ifdef SUPPORT_PEDOMETER
637 {
638 unsigned long now;
639 // pedometer is not event based, therefore we poll using a timer every
640 // pedometer_poll_timeout milliseconds
641 if ((now = inv_get_tick_count()) - last_pedometer_poll
642 > pedometer_poll_timeout) {
643 pedometer_handler();
644 last_pedometer_poll = now;
645 }
646 }
647 #endif
648 }
649
close_fds(void)650 int close_fds(void)
651 {
652 int i;
653 for (i = 0; i < NUM_DMP_FEATS; i++) {
654 if (!pfd[i].fd)
655 close(pfd[i].fd);
656 }
657 return 0;
658 }
659
660 /*******************************************************************************
661 * M a i n
662 ******************************************************************************/
663
main(int argc,char ** argv)664 int main(int argc, char **argv)
665 {
666 char data[4];
667 int i, res= 0;
668
669 printf("\n"
670 "****************************************************************\n"
671 "*** NOTE: ***\n"
672 "*** the HAL must be compiled with Low power quaternion ***\n"
673 "*** and/or DMP screen orientation support. ***\n"
674 "*** 'At least' one of the 4 Android virtual sensors ***\n"
675 "*** must be enabled. ***\n"
676 "*** ***\n"
677 "*** Please perform gestures to see the output. ***\n"
678 "*** Press any key to stop the program. ***\n"
679 "****************************************************************\n"
680 "\n");
681
682 res = inv_init_sysfs_attributes();
683 if (res) {
684 printf("GT:ERR-Can't allocate mem\n");
685 return -1;
686 }
687
688 /* check if Android Hub */
689 is_android_hub();
690
691 /* init Fds to poll for gesture data */
692 init_fds();
693
694 /* on Gesture/DMP supported features */
695 if (enable_dmp_features(1) < 0) {
696 printf("GT:ERR-Can't enable Gestures\n");
697 return -1;
698 }
699
700 do {
701 for (i = 0; i < NUM_DMP_FEATS; i++)
702 read(pfd[i].fd, data, 4);
703 poll(pfd, NUM_DMP_FEATS, POLL_TIME);
704 parse_events(pfd, NUM_DMP_FEATS);
705 } while (!_kbhit());
706
707 /* off Gesture/DMP supported features */
708 if (enable_dmp_features(0) < 0) {
709 printf("GT:ERR-Can't disable Gestures\n");
710 return -1;
711 }
712
713 /* release resources */
714 close_fds();
715 if (sysfs_names_ptr)
716 free(sysfs_names_ptr);
717
718 return res;
719 }
720
721