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 45struct 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 82enum { 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 102char *sysfs_names_ptr; 103#ifdef SUPPORT_PEDOMETER 104unsigned long last_pedometer_poll = 0L; 105unsigned long pedometer_poll_timeout = 500L; // .5 second 106#endif 107struct pollfd pfd[NUM_DMP_FEATS]; 108bool android_hub = false; // flag to indicate true=Hub, false=non-hub 109 110/******************************************************************************* 111 * DMP Feature Supported Functions 112 ******************************************************************************/ 113 114int 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 130int 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***************************************************/ 153int _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 173int 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 247int 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 255int 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 277int 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 287int 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 300int 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 313int 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 325int 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 337int 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 351int 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 400int 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 442int 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 466int 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 499int 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 527int 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 563int 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 601void 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 650int 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 664int 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