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 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 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 ***************************************************/ 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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
