| /** |
| * 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" |
| |
| #define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */ |
| |
| #define MAX_SYSFS_NAME_LEN (100) |
| #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) |
| |
| #define FLICK_UPPER_THRES 3147790 |
| #define FLICK_LOWER_THRES -3147790 |
| #define FLICK_COUNTER 50 |
| #define POLL_TIME 2000 // 2sec |
| |
| #define FALSE 0 |
| #define TRUE 1 |
| |
| char *sysfs_names_ptr; |
| |
| struct sysfs_attrbs { |
| char *enable; |
| char *power_state; |
| char *dmp_on; |
| char *dmp_int_on; |
| char *self_test; |
| char *dmp_firmware; |
| char *firmware_loaded; |
| char *display_orientation_on; |
| char *orientation_on; |
| char *event_flick; |
| char *event_display_orientation; |
| char *event_orientation; |
| char *event_tap; |
| char *flick_axis; |
| char *flick_counter; |
| char *flick_int_on; |
| char *flick_lower; |
| char *flick_upper; |
| char *flick_message_on; |
| char *tap_min_count; |
| char *tap_on; |
| char *tap_threshold; |
| char *tap_time; |
| } mpu; |
| |
| enum { |
| tap, |
| flick, |
| gOrient, |
| orient, |
| numDMPFeatures |
| }; |
| |
| struct pollfd pfd[numDMPFeatures]; |
| |
| /******************************************************************************* |
| * 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 { |
| MPL_LOGE("ERR open file to read"); |
| res= -1; |
| } |
| return res; |
| } |
| |
| int write_sysfs_int(char *filename, int data) |
| { |
| int res=0; |
| FILE *fp; |
| |
| fp = fopen(filename, "w"); |
| if (fp!=NULL) { |
| fprintf(fp, "%d\n", data); |
| fclose(fp); |
| } else { |
| MPL_LOGE("ERR open file to write"); |
| res= -1; |
| } |
| return res; |
| } |
| |
| /************************************************** |
| This _kbhit() function is courtesy from Web |
| ***************************************************/ |
| int _kbhit() { |
| 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 { |
| MPL_LOGE("couldn't alloc mem for sysfs paths"); |
| return -1; |
| } |
| |
| // get proper (in absolute/relative) IIO path & build MPU's sysfs paths |
| inv_get_sysfs_path(sysfs_path); |
| |
| 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.self_test, "%s%s", sysfs_path, "/self_test"); |
| sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware"); |
| sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded"); |
| sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on"); |
| sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on"); |
| sprintf(mpu.event_flick, "%s%s", sysfs_path, "/event_flick"); |
| sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation"); |
| sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation"); |
| sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap"); |
| sprintf(mpu.flick_axis, "%s%s", sysfs_path, "/flick_axis"); |
| sprintf(mpu.flick_counter, "%s%s", sysfs_path, "/flick_counter"); |
| sprintf(mpu.flick_int_on, "%s%s", sysfs_path, "/flick_int_on"); |
| sprintf(mpu.flick_lower, "%s%s", sysfs_path, "/flick_lower"); |
| sprintf(mpu.flick_upper, "%s%s", sysfs_path, "/flick_upper"); |
| sprintf(mpu.flick_message_on, "%s%s", sysfs_path, "/flick_message_on"); |
| 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"); |
| |
| #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 DmpFWloaded() |
| { |
| int res; |
| read_sysfs_int(mpu.firmware_loaded, &res); |
| return res; |
| } |
| |
| int enable_flick(int en) |
| { |
| int res=0; |
| int flickUpper=0, flickLower=0, flickCounter=0; |
| |
| if (write_sysfs_int(mpu.flick_int_on, en) < 0) { |
| printf("GT:ERR-can't write 'flick_int_on'"); |
| res= -1; |
| } |
| |
| if (en) { |
| flickUpper= FLICK_UPPER_THRES; |
| flickLower= FLICK_LOWER_THRES; |
| flickCounter= FLICK_COUNTER; |
| } |
| |
| if (write_sysfs_int(mpu.flick_upper, flickUpper) < 0) { |
| printf("GT:ERR-can't write 'flick_upper'"); |
| res= -1; |
| } |
| |
| if (write_sysfs_int(mpu.flick_lower, flickLower) < 0) { |
| printf("GT:ERR-can't write 'flick_lower'"); |
| res= -1; |
| } |
| |
| if (write_sysfs_int(mpu.flick_counter, flickCounter) < 0) { |
| printf("GT:ERR-can't write 'flick_counter'"); |
| res= -1; |
| } |
| |
| if (write_sysfs_int(mpu.flick_message_on, 0) < 0) { |
| printf("GT:ERR-can't write 'flick_message_on'"); |
| res= -1; |
| } |
| |
| if (write_sysfs_int(mpu.flick_axis, 0) < 0) { |
| printf("GT:ERR_can't write 'flick_axis'"); |
| res= -1; |
| } |
| |
| return res; |
| } |
| |
| int enable_tap(int en) |
| { |
| if (write_sysfs_int(mpu.tap_on, en) < 0) { |
| printf("GT:ERR-can't write 'tap_on'"); |
| return -1; |
| } |
| |
| return 0; |
| } |
| |
| int enable_displ_orient(int en) |
| { |
| if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { |
| printf("GT:ERR-can't write 'display_orientation_en'"); |
| return -1; |
| } |
| |
| return 0; |
| } |
| |
| int enable_orient(int en) |
| { |
| if (write_sysfs_int(mpu.orientation_on, en) < 0) { |
| printf("GT:ERR-can't write 'orientation_on'"); |
| return -1; |
| } |
| |
| return 0; |
| } |
| |
| int flickHandler() |
| { |
| FILE *fp; |
| int data; |
| |
| #ifdef DEBUG_PRINT |
| printf("GT:Flick Handler\n"); |
| #endif |
| |
| fp = fopen(mpu.event_flick, "rt"); |
| fscanf(fp, "%d\n", &data); |
| fclose (fp); |
| |
| printf("Flick= %x\n", data); |
| |
| return 0; |
| } |
| |
| int tapHandler() |
| { |
| 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\n"); |
| break; |
| case 2: |
| printf("Tap Axis->X Neg\n"); |
| break; |
| case 3: |
| printf("Tap Axis->Y Pos\n"); |
| break; |
| case 4: |
| printf("Tap Axis->Y Neg\n"); |
| break; |
| case 5: |
| printf("Tap Axis->Z Pos\n"); |
| break; |
| case 6: |
| printf("Tap Axis->Z Neg\n"); |
| break; |
| default: |
| printf("Tap Axis->Unknown\n"); |
| break; |
| } |
| |
| return 0; |
| } |
| |
| int googleOrientHandler() |
| { |
| FILE *fp; |
| int orient; |
| |
| #ifdef DEBUG_PRINT |
| printf("GT:Google Orient Handler\n"); |
| #endif |
| |
| fp = fopen(mpu.event_display_orientation, "rt"); |
| fscanf(fp, "%d\n", &orient); |
| fclose(fp); |
| |
| printf("Google Orient-> %d\n", orient); |
| |
| return 0; |
| } |
| |
| int orientHandler() |
| { |
| 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; |
| } |
| |
| int enableDMPFeatures(int en) |
| { |
| int res= -1; |
| |
| if (DmpFWloaded()) |
| { |
| /* 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 */ |
| |
| enable_flick(en); |
| enable_tap(en); |
| enable_displ_orient(en); |
| enable_orient(en); |
| res= 0; |
| } |
| |
| return res; |
| } |
| |
| int initFds() |
| { |
| int i; |
| |
| for (i=0; i< numDMPFeatures; i++) { |
| switch(i) { |
| case tap: |
| pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK); |
| break; |
| |
| case flick: |
| pfd[i].fd = open(mpu.event_flick, O_RDONLY | O_NONBLOCK); |
| break; |
| |
| case gOrient: |
| pfd[i].fd = open(mpu.event_display_orientation, O_RDONLY | O_NONBLOCK); |
| break; |
| |
| case orient: |
| pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK); |
| break; |
| |
| default: |
| pfd[i].fd = -1; |
| } |
| |
| pfd[i].events = POLLPRI|POLLERR, |
| pfd[i].revents = 0; |
| #ifdef DEBUG_PRINT |
| printf("GT:pfd[%d].fd= %d\n", i, pfd[i].fd); |
| #endif |
| } |
| |
| return 0; |
| } |
| |
| int closeFds() |
| { |
| int i; |
| for (i = 0; i < numDMPFeatures; i++) { |
| if (!pfd[i].fd) |
| close(pfd[i].fd); |
| } |
| return 0; |
| } |
| |
| /******************************************************************************* |
| * M a i n S e l f T e s t |
| ******************************************************************************/ |
| |
| int main(int argc, char **argv) |
| { |
| char data[4]; |
| int i, res= 0; |
| |
| res = inv_init_sysfs_attributes(); |
| if (res) { |
| printf("GT:ERR-Can't allocate mem"); |
| return -1; |
| } |
| |
| /* On Gesture/DMP supported features */ |
| enableDMPFeatures(1); |
| |
| /* init Fds to poll for Gesture data */ |
| initFds(); |
| |
| /* prompt user to make gesture and how to stop program */ |
| printf("\n**Please make Gesture to see data. Press any key to stop Prog**\n\n"); |
| |
| do { |
| for (i=0; i< numDMPFeatures; i++) { |
| read(pfd[i].fd, data, 4); |
| } |
| |
| poll(pfd, numDMPFeatures, POLL_TIME); |
| |
| for (i=0; i< numDMPFeatures; i++) { |
| if(pfd[i].revents != 0) { |
| switch(i) { |
| case tap: |
| tapHandler(); |
| break; |
| |
| case flick: |
| flickHandler(); |
| break; |
| |
| case gOrient: |
| googleOrientHandler(); |
| break; |
| |
| case orient: |
| orientHandler(); |
| break; |
| |
| default: |
| printf("GT:ERR-Not supported"); |
| break; |
| } |
| pfd[i].revents= 0; //no need. reset anyway |
| } |
| } |
| |
| } while (!_kbhit()); |
| |
| /* Off DMP features */ |
| enableDMPFeatures(0); |
| |
| /* release resources */ |
| closeFds(); |
| if (sysfs_names_ptr) { |
| free(sysfs_names_ptr); |
| } |
| |
| printf("\nThank You!\n"); |
| |
| return res; |
| } |
| |