diff options
Diffstat (limited to '6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c')
| -rw-r--r-- | 6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c | 720 |
1 files changed, 720 insertions, 0 deletions
diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c new file mode 100644 index 0000000..ac0803f --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c @@ -0,0 +1,720 @@ +/** + * 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; +} + |