xref: /hardware/invensense/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c

/**
 *  Gesture Test application for Invensense's MPU6/9xxx (w/ DMP).
 */

#include <unistd.h>
#include <dirent.h>
#include <fcntl.h>
#include <stdio.h>
#include <errno.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <features.h>
#include <dirent.h>
#include <string.h>
#include <poll.h>
#include <stddef.h>
#include <linux/input.h>
#include <time.h>
#include <linux/time.h>
#include <unistd.h>
#include <termios.h>

#include "invensense.h"
#include "ml_math_func.h"
#include "storage_manager.h"
#include "ml_stored_data.h"
#include "ml_sysfs_helper.h"
#include "mlos.h"

//#define DEBUG_PRINT   /* Uncomment to print Gyro & Accel read from Driver */

#define SUPPORT_SCREEN_ORIENTATION
//#define SUPPORT_TAP
//#define SUPPORT_ORIENTATION
#define SUPPORT_PEDOMETER
#define SUPPORT_SMD

#define MAX_SYSFS_NAME_LEN  (100)
#define MAX_SYSFS_ATTRB     (sizeof(struct sysfs_attrbs) / sizeof(char*))
#define IIO_SYSFS_PATH      "/sys/bus/iio/devices/iio:device0"
#define IIO_HUB_NAME        "inv_hub"

#define POLL_TIME           (2000) // 2sec

struct sysfs_attrbs {
    char *name;
    char *enable;
    char *power_state;
    char *dmp_on;
    char *dmp_int_on;
    char *dmp_firmware;
    char *firmware_loaded;
#ifdef SUPPORT_SCREEN_ORIENTATION
    char *event_display_orientation;
    char *display_orientation_on;
#endif
#ifdef SUPPORT_ORIENTATION
    char *event_orientation;
    char *orientation_on;
#endif
#ifdef SUPPORT_TAP
    char *event_tap;
    char *tap_min_count;
    char *tap_on;
    char *tap_threshold;
    char *tap_time;
#endif
#ifdef SUPPORT_PEDOMETER
    char *pedometer_on;
    char *pedometer_steps;
    char *pedometer_time;
#endif
#ifdef SUPPORT_SMD
    char *event_smd;
    char *smd_enable;
    char *smd_threshold;
    char *smd_delay_threshold;
    char *smd_delay_threshold2;
#endif
} mpu;

enum {
#ifdef SUPPORT_TAP
    FEAT_TAP,
#endif
#ifdef SUPPORT_SCREEN_ORIENTATION
    FEAT_SCREEN_ORIENTATION,
#endif
#ifdef SUPPORT_ORIENTATION
    FEAT_ORIENTATION,
#endif
#ifdef SUPPORT_PEDOMETER
    FEAT_PEDOMETER,
#endif
#ifdef SUPPORT_SMD
    FEAT_SMD,
#endif

    NUM_DMP_FEATS
};

char *sysfs_names_ptr;
#ifdef SUPPORT_PEDOMETER
unsigned long last_pedometer_poll = 0L;
unsigned long pedometer_poll_timeout = 500L; // .5 second
#endif
struct pollfd pfd[NUM_DMP_FEATS];
bool android_hub = false;   // flag to indicate true=Hub, false=non-hub

/*******************************************************************************
 *                       DMP Feature Supported Functions
 ******************************************************************************/

int read_sysfs_int(char *filename, int *var)
{
    int res=0;
    FILE *fp;

    fp = fopen(filename, "r");
    if (fp!=NULL) {
        fscanf(fp, "%d\n", var);
        fclose(fp);
    } else {
        printf("ERR open file to read: %s\n", filename);
        res= -1;
    }
    return res;
}

int write_sysfs_int(char *filename, int data)
{
    int res=0;
    FILE  *fp;

#ifdef DEBUG_PRINT
    printf("writing '%s' with '%d'\n", filename, data);
#endif

    fp = fopen(filename, "w");
    if (fp != NULL) {
        fprintf(fp, "%d\n", data);
        fclose(fp);
    } else {
        printf("ERR open file to write: %s\n", filename);
        res = -1;
    }
    return res;
}

/**************************************************
    This _kbhit() function is courtesy of the web
***************************************************/
int _kbhit(void)
{
    static const int STDIN = 0;
    static bool initialized = false;

    if (! initialized) {
        // Use termios to turn off line buffering
        struct termios term;
        tcgetattr(STDIN, &term);
        term.c_lflag &= ~ICANON;
        tcsetattr(STDIN, TCSANOW, &term);
        setbuf(stdin, NULL);
        initialized = true;
    }

    int bytesWaiting;
    ioctl(STDIN, FIONREAD, &bytesWaiting);
    return bytesWaiting;
}

int inv_init_sysfs_attributes(void)
{
    unsigned char i = 0;
    char sysfs_path[MAX_SYSFS_NAME_LEN];
    char *sptr;
    char **dptr;

    sysfs_names_ptr =
            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
    sptr = sysfs_names_ptr;
    if (sptr != NULL) {
        dptr = (char**)&mpu;
        do {
            *dptr++ = sptr;
            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
        } while (++i < MAX_SYSFS_ATTRB);
    } else {
        printf("couldn't alloc mem for sysfs paths\n");
        return -1;
    }

    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
    inv_get_sysfs_path(sysfs_path);

    sprintf(mpu.name, "%s%s", sysfs_path, "/name");
    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");

#ifdef SUPPORT_SCREEN_ORIENTATION
    sprintf(mpu.event_display_orientation, "%s%s",
            sysfs_path, "/event_display_orientation");
    sprintf(mpu.display_orientation_on, "%s%s",
            sysfs_path, "/display_orientation_on");
#endif
#ifdef SUPPORT_ORIENTATION
    sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
    sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
#endif
#ifdef SUPPORT_TAP
    sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
    sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
    sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
    sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
#endif
#ifdef SUPPORT_PEDOMETER
    sprintf(mpu.pedometer_on, "%s%s", sysfs_path, "/dmp_on");
    sprintf(mpu.pedometer_steps, "%s%s", sysfs_path, "/pedometer_steps");
    sprintf(mpu.pedometer_time, "%s%s", sysfs_path, "/pedometer_time");
#endif
#ifdef SUPPORT_SMD
    sprintf(mpu.event_smd, "%s%s", sysfs_path, "/event_smd");
    sprintf(mpu.smd_enable, "%s%s", sysfs_path, "/smd_enable");
    sprintf(mpu.smd_threshold, "%s%s", sysfs_path, "/smd_threshold");
    sprintf(mpu.smd_delay_threshold, "%s%s",
            sysfs_path, "/smd_delay_threshold");
    sprintf(mpu.smd_delay_threshold2, "%s%s",
            sysfs_path, "/smd_delay_threshold2");
#endif

#if 0
    // test print sysfs paths
    dptr = (char**)&mpu;
    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
        MPL_LOGE("sysfs path: %s", *dptr++);
    }
#endif
    return 0;
}

int dmp_fw_loaded(void)
{
    int fw_loaded;
    if (read_sysfs_int(mpu.firmware_loaded, &fw_loaded) < 0)
        fw_loaded= 0;
    return fw_loaded;
}

int is_android_hub(void)
{
    char dev_name[8];
    FILE *fp;

    fp= fopen(mpu.name, "r");
    fgets(dev_name, 8, fp);
    fclose(fp);

    if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME))) {
        android_hub = true;
    }else {
        android_hub = false;
    }

    return 0;
}

/*
    Enablers for the gestures
*/

int master_enable(int en)
{
    if (write_sysfs_int(mpu.enable, en) < 0) {
        printf("GT:ERR-can't write 'buffer/enable'");
        return -1;
    }
    return 0;
}

#ifdef SUPPORT_TAP
int enable_tap(int en)
{
    if (write_sysfs_int(mpu.tap_on, en) < 0) {
        printf("GT:ERR-can't write 'tap_on'\n");
        return -1;
    }

    return 0;
}
#endif

/* Unnecessary: pedometer_on == dmp_on, which is always on
#ifdef SUPPORT_PEDOMETER
int enable_pedometer(int en)
{
    if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
        printf("GT:ERR-can't write 'pedometer_on'\n");
        return -1;
    }

    return 0;
}
#endif
*/

#ifdef SUPPORT_SCREEN_ORIENTATION
int enable_display_orientation(int en)
{
    if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
        printf("GT:ERR-can't write 'display_orientation_on'\n");
        return -1;
    }

    return 0;
}
#endif

#ifdef SUPPORT_ORIENTATION
int enable_orientation(int en)
{
    if (write_sysfs_int(mpu.orientation_on, en) < 0) {
        printf("GT:ERR-can't write 'orientation_on'\n");
        return -1;
    }

    return 0;
}
#endif

#ifdef SUPPORT_SMD
int enable_smd(int en)
{
    if (write_sysfs_int(mpu.smd_enable, en) < 0) {
        printf("GT:ERR-can't write 'smd_enable'\n");
        return -1;
    }
    return 0;
}
#endif

/*
    Handlers for the gestures
*/
#ifdef SUPPORT_TAP
int tap_handler(void)
{
    FILE *fp;
    int tap, tap_dir, tap_num;

    fp = fopen(mpu.event_tap, "rt");
    fscanf(fp, "%d\n", &tap);
    fclose(fp);

    tap_dir = tap/8;
    tap_num = tap%8 + 1;

#ifdef DEBUG_PRINT
    printf("GT:Tap Handler **\n");
    printf("Tap= %x\n", tap);
    printf("Tap Dir= %x\n", tap_dir);
    printf("Tap Num= %x\n", tap_num);
#endif

    switch (tap_dir) {
        case 1:
            printf("Tap Axis->X Pos, ");
            break;
        case 2:
            printf("Tap Axis->X Neg, ");
            break;
        case 3:
            printf("Tap Axis->Y Pos, ");
            break;
        case 4:
            printf("Tap Axis->Y Neg, ");
            break;
        case 5:
            printf("Tap Axis->Z Pos, ");
            break;
        case 6:
            printf("Tap Axis->Z Neg, ");
            break;
        default:
            printf("Tap Axis->Unknown, ");
            break;
    }
    printf("#%d\n", tap_num);

    return 0;
}
#endif

#ifdef SUPPORT_PEDOMETER
int pedometer_handler(void)
{
    FILE *fp;
    static int last_pedometer_steps = -1;
    static long last_pedometer_time = -1;
    int pedometer_steps;
    long pedometer_time;

#ifdef DEBUG_PRINT
    printf("GT:Pedometer Handler\n");
#endif

    fp = fopen(mpu.pedometer_steps, "rt");
    fscanf(fp, "%d\n", &pedometer_steps);
    fclose(fp);

    fp = fopen(mpu.pedometer_time, "rt");
    fscanf(fp, "%ld\n", &pedometer_time);
    fclose(fp);

    if (last_pedometer_steps == -1 && last_pedometer_time == -1) {
        printf("Pedometer Steps: %d Time: %ld ",
               pedometer_steps, pedometer_time);
        if (pedometer_steps > 10
                || pedometer_time > (pedometer_poll_timeout * 2))
            printf("(resumed)\n");
        else
            printf("\n");
    } else if (last_pedometer_steps != pedometer_steps
                    || last_pedometer_time != pedometer_time) {
    printf("Pedometer Steps: %d Time: %ld\n",
           pedometer_steps, pedometer_time);
    }

    last_pedometer_steps = pedometer_steps;
    last_pedometer_time = pedometer_time;

    return 0;
}
#endif

#ifdef SUPPORT_SCREEN_ORIENTATION
int display_orientation_handler(void)
{
    FILE *fp;
    int orient;

#ifdef DEBUG_PRINT
    printf("GT:Screen Orient Handler\n");
#endif

    fp = fopen(mpu.event_display_orientation, "rt");
    if (!fp) {
        printf("GT:Cannot open '%s'\n", mpu.event_display_orientation);
        return -1;
    }
    fscanf(fp, "%d\n", &orient);
    fclose(fp);

    printf("Screen Orient-> %d\n", orient);

    return 0;
}
#endif

#ifdef SUPPORT_ORIENTATION
int host_orientation_handler(void)
{
    FILE *fp;
    int orient;

    fp = fopen(mpu.event_orientation, "rt");
    fscanf(fp, "%d\n", &orient);
    fclose(fp);

#ifdef DEBUG_PRINT
    printf("GT:Reg Orient Handler\n");
#endif

    if (orient & 0x01)
        printf("Orient->X Up\n");
    if (orient & 0x02)
        printf("Orient->X Down\n");
    if (orient & 0x04)
        printf("Orient->Y Up\n");
    if (orient & 0x08)
        printf("Orient->Y Down\n");
    if (orient & 0x10)
        printf("Orient->Z Up\n");
    if (orient & 0x20)
        printf("Orient->Z Down\n");
    if (orient & 0x40)
        printf("Orient->Flip\n");

    return 0;
}
#endif

#ifdef SUPPORT_SMD
int smd_handler(void)
{
    FILE *fp;
    int smd;

    fp = fopen(mpu.event_smd, "rt");
    fscanf(fp, "%d\n", &smd);
    fclose(fp);

#ifdef DEBUG_PRINT
    printf("GT:SMD Handler\n");
#endif
    printf("SMD (%d)\n", smd);

    /* wait for the acceleration low pass filtered tail to die off -
       this is to prevent that the tail end of a 2nd event of above threhsold
       motion be considered as also the 1st event for the next SM detection */
    inv_sleep(1000);

    /* re-enable to continue the detection */
    master_enable(0);
    enable_smd(1);
    master_enable(1);

    return 0;
}
#endif

int enable_dmp_features(int en)
{
    int res= -1;

    if (android_hub || dmp_fw_loaded()) {
        /* Currently there's no info regarding DMP's supported features/capabilities
           An error in enabling features below could be an indication of the feature
           not supported in current loaded DMP firmware */

        master_enable(0);
#ifdef SUPPORT_TAP
        enable_tap(en);
#endif
#ifdef SUPPORT_SCREEN_ORIENTATION
        enable_display_orientation(en);
#endif
#ifdef SUPPORT_ORIENTATION
        if (android_hub == false) {
            // Android Hub does not support 'regular' orientation feature
            enable_orientation(en);
        }
#endif
#ifdef SUPPORT_SMD
        enable_smd(en);
#endif
        master_enable(1);
        res = 0;

    } else {
        printf("GT:ERR-No DMP firmware\n");
        res= -1;
    }

    return res;
}

int init_fds(void)
{
    int i;

    for (i = 0; i < NUM_DMP_FEATS; i++) {
        switch(i) {
#ifdef SUPPORT_TAP
        case FEAT_TAP:
            pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
            break;
#endif
#ifdef SUPPORT_SCREEN_ORIENTATION
        case FEAT_SCREEN_ORIENTATION:
            pfd[i].fd = open(mpu.event_display_orientation,
                             O_RDONLY | O_NONBLOCK);
            break;
#endif
#ifdef SUPPORT_ORIENTATION
        case FEAT_ORIENTATION:
            pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
            break;
#endif
#ifdef SUPPORT_SMD
        case FEAT_SMD:
            pfd[i].fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
            break;
#endif
        default:
            pfd[i].fd = -1;
        }

        pfd[i].events = POLLPRI|POLLERR,
        pfd[i].revents = 0;
    }

    return 0;
}

void parse_events(struct pollfd pfd[], int num_fds)
{
    int i;

    for (i = 0; i < num_fds; i++) {
        if(pfd[i].revents != 0) {
            switch(i) {
#ifdef SUPPORT_TAP
            case FEAT_TAP:
                tap_handler();
                break;
#endif
#ifdef SUPPORT_SCREEN_ORIENTATION
            case FEAT_SCREEN_ORIENTATION:
                display_orientation_handler();
                break;
#endif
#ifdef SUPPORT_ORIENTATION
            case FEAT_ORIENTATION:
                host_orientation_handler();
                break;
#endif
#ifdef SUPPORT_SMD
            case FEAT_SMD:
                smd_handler();
                break;
#endif
            default:
                printf("GT:ERR-unhandled/unrecognized gesture event");
                break;
            }
            pfd[i].revents = 0;   // no need: reset anyway
        }
    }

#ifdef SUPPORT_PEDOMETER
    {
        unsigned long now;
        // pedometer is not event based, therefore we poll using a timer every
        //  pedometer_poll_timeout milliseconds
        if ((now = inv_get_tick_count()) - last_pedometer_poll
                > pedometer_poll_timeout) {
            pedometer_handler();
            last_pedometer_poll = now;
        }
    }
#endif
}

int close_fds(void)
{
    int i;
    for (i = 0; i < NUM_DMP_FEATS; i++) {
        if (!pfd[i].fd)
            close(pfd[i].fd);
    }
    return 0;
}

/*******************************************************************************
 *                       M a i n
 ******************************************************************************/

int main(int argc, char **argv)
{
    char data[4];
    int i, res= 0;

    printf("\n"
           "****************************************************************\n"
           "*** NOTE:                                                    ***\n"
           "***       the HAL must be compiled with Low power quaternion ***\n"
           "***           and/or DMP screen orientation support.         ***\n"
           "***       'At least' one of the 4 Android virtual sensors    ***\n"
           "***           must be enabled.                               ***\n"
           "***                                                          ***\n"
           "*** Please perform gestures to see the output.               ***\n"
           "*** Press any key to stop the program.                       ***\n"
           "****************************************************************\n"
           "\n");

    res = inv_init_sysfs_attributes();
    if (res) {
        printf("GT:ERR-Can't allocate mem\n");
        return -1;
    }

    /* check if Android Hub */
    is_android_hub();

    /* init Fds to poll for gesture data */
    init_fds();

    /* on Gesture/DMP supported features */
    if (enable_dmp_features(1) < 0) {
        printf("GT:ERR-Can't enable Gestures\n");
        return -1;
    }

    do {
        for (i = 0; i < NUM_DMP_FEATS; i++)
            read(pfd[i].fd, data, 4);
        poll(pfd, NUM_DMP_FEATS, POLL_TIME);
        parse_events(pfd, NUM_DMP_FEATS);
    } while (!_kbhit());

    /* off Gesture/DMP supported features */
    if (enable_dmp_features(0) < 0) {
        printf("GT:ERR-Can't disable Gestures\n");
        return -1;
    }

    /* release resources */
    close_fds();
    if (sysfs_names_ptr)
        free(sysfs_names_ptr);

    return res;
}