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