diff options
Diffstat (limited to 'libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c')
-rw-r--r-- | libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c | 485 |
1 files changed, 485 insertions, 0 deletions
diff --git a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c new file mode 100644 index 0000000..6fa9aab --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c @@ -0,0 +1,485 @@ +/* + * input interface testing + */ +#include <sys/time.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <time.h> +#include <fcntl.h> +#include <sys/ioctl.h> +#include <linux/input.h> +#include <linux/config.h> +#include <stdio.h> +#include <stdlib.h> +#include "linux/ml_sysfs_helper.h" +#include "authenticate.h" +#include "ml_load_dmp.h" + +#if 0 +struct input_event { + struct timeval time; + __u16 type; + __u16 code; + __s32 value; +}; +#endif + +void HandleOrient(int orient) +{ + if (orient & 0x01) + printf("INV_X_UP\n"); + if (orient & 0x02) + printf("INV_X_DOWN\n"); + if (orient & 0x04) + printf("INV_Y_UP\n"); + if (orient & 0x08) + printf("INV_Y_DOWN\n"); + if (orient & 0x10) + printf("INV_Z_UP\n"); + if (orient & 0x20) + printf("INV_Z_DOWN\n"); + if (orient & 0x40) + printf("INV_ORIENTATION_FLIP\n"); +} + +void HandleTap(int tap) +{ + int tap_dir = tap/8; + int tap_num = tap%8 + 1; + + switch (tap_dir) { + case 1: + printf("INV_TAP_AXIS_X_POS\n"); + break; + case 2: + printf("INV_TAP_AXIS_X_NEG\n"); + break; + case 3: + printf("INV_TAP_AXIS_Y_POS\n"); + break; + case 4: + printf("INV_TAP_AXIS_Y_NEG\n"); + break; + case 5: + printf("INV_TAP_AXIS_Z_POS\n"); + break; + case 6: + printf("INV_TAP_AXIS_Z_NEG\n"); + break; + default: + break; + } + printf("Tap number: %d\n", tap_num); +} + +static void read_compass(int event_number) +{ + int ev_size, ret_byte, ii; + int fd = -1; + struct input_event ev[10]; + char name[64]; + char file_name[64]; + unsigned int RX; + unsigned long long time0, time1, time2; + struct timespec tsp; + ev_size = sizeof(struct input_event); + sprintf(file_name, "/dev/input/event%d", event_number); + if ((fd = open(file_name, O_RDONLY)) < 0 ) { + printf("fail to open compass\n"); + return; + } + + /* NOTE: Use this to pass device name to HAL. */ + ioctl (fd, EVIOCGNAME (sizeof (name)), name); + printf ("Reading From : (%s)\n", name); + while (1) { + clock_gettime(CLOCK_MONOTONIC, &tsp); + /*read compass data here */ + if(fd > 0){ + ret_byte = read(fd, ev, ev_size); + } else { + ret_byte = -1; + } + time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL; + if (ret_byte < 0) + continue; + for (ii = 0; ii < ret_byte/ev_size; ii++) { + if(EV_REL != ev[ii].type) { + time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL; + printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0)); + continue; + } + switch (ev[ii].code) { + case REL_X: + printf("CX:%5d ", ev[ii].value); + break; + case REL_Y: + printf("CY:%5d ", ev[ii].value); + break; + case REL_Z: + printf("CZ:%5d ", ev[ii].value); + break; + case REL_MISC: + RX = ev[ii].value; + break; + case REL_WHEEL: + time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value; + time1 = time1/1000000; + printf("time1: %lld ", time1); + break; + default: + printf("GES?: %5d ", ev[ii].code); + break; + } + } + } + close(fd); +} + +static void read_gesture(int num) +{ + int ev_size, ret_byte, ii; + int fd = -1; + struct input_event ev[10]; + char name[64]; + char file_name[64]; + unsigned long long time; + struct timespec tsp; + ev_size = sizeof(struct input_event); + sprintf(file_name, "/dev/input/event%d", num); + MPL_LOGI("%s\n", file_name); + if ((fd = open(file_name, O_RDONLY)) < 0 ) { + printf("fail to open gusture.\n"); + return; + } + + /* NOTE: Use this to pass device name to HAL. */ + ioctl (fd, EVIOCGNAME (sizeof (name)), name); + printf ("Reading From : (%s)\n", name); + while(1){ + clock_gettime(CLOCK_MONOTONIC, &tsp); + if(fd > 0){ + ret_byte = read(fd, ev, ev_size); + } else { + ret_byte = -1; + } + time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL; + //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3); + if (ret_byte < 0) + continue; + for (ii = 0; ii < ret_byte/ev_size; ii++) { + if(EV_REL != ev[ii].type) { + time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL; + printf("mono=%lld\n", time); + continue; + } + switch (ev[ii].code) { + case REL_RX: + printf("GESX:%5x\n", ev[ii].value); + HandleTap(ev[ii].value); + break; + case REL_RY: + printf("GESY:%5x\n", ev[ii].value); + HandleOrient(ev[ii].value); + break; + case REL_RZ: + printf("FLICK:%5x\n", ev[ii].value); + break; + default: + printf("?: %5d ", ev[ii].code); + break; + } + } + } +} + +static void read_gyro_accel(int num) +{ + int ev_size, ret_byte, ii; + int fd = -1; + unsigned int RX; + struct input_event ev[10]; + char name[64]; + char file_name[64]; + unsigned long long time0, time1, time2; + struct timespec tsp; + ev_size = sizeof(struct input_event); + sprintf(file_name, "/dev/input/event%d", num); + if ((fd = open(file_name, O_RDONLY)) < 0 ) { + printf("fail to open gyro/accel\n"); + return; + } + + /* NOTE: Use this to pass device name to HAL. */ + ioctl (fd, EVIOCGNAME (sizeof (name)), name); + printf ("Reading From : (%s)\n", name); + while (1){ + //usleep(20000); + ret_byte = read(fd, ev, ev_size); + if (ret_byte < 0) + continue; + //ret_byte = 0; + + for (ii = 0; ii < ret_byte/ev_size; ii++) { + if(EV_REL != ev[ii].type) { + time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL; + printf("T: %lld diff=%d ", time0, (int)(time1 - time0)); + clock_gettime(CLOCK_MONOTONIC, &tsp); + time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL; + printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2)); + continue; + } + switch (ev[ii].code) { + case REL_X: + printf("GX:%5d ", ev[ii].value); + break; + case REL_Y: + printf("GY:%5d ", ev[ii].value); + break; + case REL_Z: + printf("GZ:%5d ", ev[ii].value); + break; + case REL_RX: + printf("AX:%5d ", ev[ii].value); + break; + case REL_RY: + printf("AY:%5d ", ev[ii].value); + break; + case REL_RZ: + printf("AZ:%5d ", ev[ii].value); + break; + case REL_MISC: + RX = ev[ii].value; + break; + case REL_WHEEL: + time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value; + time1 = time1/1000000; + printf("time1: %lld ", time1); + break; + default: + printf("?: %5d ", ev[ii].code); + break; + } + } + } + close(fd); +} +int inv_sysfs_write(char *filename, long data) +{ + FILE *fp; + int count; + + if (!filename) + return -1; + fp = fopen(filename, "w"); + if (!fp) + return -errno; + count = fprintf(fp, "%ld", data); + fclose(fp); + return count; +} +int inv_sysfs_read(char *filename, long num_bytes, char *data) +{ + FILE *fp; + int count; + + if (!filename) + return -1; + fp = fopen(filename, "r"); + if (!fp) + return -errno; + count = fread(data, 1, num_bytes, fp); + fclose(fp); + return count; +} + +void enable_flick(char *p) +{ + char sysfs_file[200]; + printf("flick:%s\n", p); + sprintf(sysfs_file, "%s/flick_int_on", p); + inv_sysfs_write(sysfs_file, 1); + sprintf(sysfs_file, "%s/flick_upper", p); + inv_sysfs_write(sysfs_file, 3147790); + sprintf(sysfs_file, "%s/flick_lower", p); + inv_sysfs_write(sysfs_file, -3147790); + sprintf(sysfs_file, "%s/flick_counter", p); + inv_sysfs_write(sysfs_file, 50); + sprintf(sysfs_file, "%s/flick_message_on", p); + inv_sysfs_write(sysfs_file, 0); + sprintf(sysfs_file, "%s/flick_axis", p); + inv_sysfs_write(sysfs_file, 2); +} + +void setup_dmp(char *sysfs_path) +{ + char sysfs_file[200]; + char firmware_loaded[200], dmp_path[200]; + char dd[10]; + + inv_get_dmpfile(dmp_path); + sprintf(sysfs_file, "%s/fifo_rate", sysfs_path); + inv_sysfs_write(sysfs_file, 200); + sprintf(sysfs_file, "%s/FSR", sysfs_path); + inv_sysfs_write(sysfs_file, 2000); + sprintf(sysfs_file, "%s/accl_fs", sysfs_path); + inv_sysfs_write(sysfs_file, 4); + /* + sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded"); + printf("%s\n", firmware_loaded); + inv_sysfs_write(firmware_loaded, 0); + inv_sysfs_read(firmware_loaded, 1, dd); + printf("beforefirmware_loaded=%c\n", dd[0]); + + if ((fd = open(dmp_path, O_WRONLY)) < 0 ) { + perror("dmp fail"); + } + inv_load_dmp(fd); + close(fd); + */ + inv_sysfs_read(firmware_loaded, 1, dd); + printf("firmware_loaded=%c\n", dd[0]); +} +void read_pedometer(char *sysfs_path){ + int steps; + char sysfs_file[200]; + char dd[4]; + sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path); + inv_sysfs_read(sysfs_file, 4, dd); + steps = dd[0] << 8 | dd[1]; + printf("fff=%d\n", steps); +} +/* The running sequence: + "input_gyro 2 &". + This will setup the dmp firmware and let it run on background. + tap and flick will work at this time. + To see accelerometer data and gyro data. + type : + "input_gyro ". + This will print out gyro data and accelerometer data + To see Compass data + type: + "input_gyro 1" */ + +int main(int argc, char *argv[]) +{ + unsigned int RX, i, sel; + unsigned char key[16]; + struct timeval tv; + struct timespec tsp0, tsp1, tsp2, tsp3; + int event_num; + char sysfs_path[200]; + char chip_name[20]; + char sysfs_file[200]; + if (INV_SUCCESS != inv_check_key()) { + printf("key check fail\n"); + exit(0); + }else + printf("key authenticated\n"); + + for(i=0;i<16;i++){ + key[i] = 0xff; + } + RX = inv_get_sysfs_key(key); + if(RX == INV_SUCCESS){ + for(i=0;i<16;i++){ + printf("%d, ", key[i]); + } + printf("\n"); + }else{ + printf("get key failed\n"); + } + memset(sysfs_path, 0, 200); + memset(sysfs_file, 0, 200); + memset(chip_name, 0, 20); + inv_get_sysfs_path(sysfs_path); + inv_get_chip_name(chip_name); + printf("sysfs path: %s\n", sysfs_path); + printf("chip name: %s\n", chip_name); + /*set up driver*/ + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 0); + sprintf(sysfs_file, "%s/power_state", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if ((getuid ()) != 0) + printf ("You are not root! This may not work...\n"); + + if(argc ==2 ) + sel = argv[1][0] - 0x30; + else + sel = 0; + switch(sel){ + case 0: + printf("-------------------------------\n"); + printf("--- log gyro and accel data ---\n"); + printf("-------------------------------\n"); + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if(inv_get_handler_number(chip_name, &event_num) < 0) + printf("mpu not installed\n"); + else + read_gyro_accel(event_num); + break; + + case 1: + printf("------------------------\n"); + printf("--- log compass data ---\n"); + printf("------------------------\n"); + sprintf(sysfs_file, "%s/compass_enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if(inv_get_handler_number("INV_COMPASS", &event_num) < 0) + printf("compass is not enabled\n"); + else + read_compass(event_num); + break; + + case 2: + printf("--------------------\n"); + printf("--- log gestures ---\n"); + printf("--------------------\n"); + setup_dmp(sysfs_path); + enable_flick(sysfs_path); + sprintf(sysfs_file, "%s/tap_on", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if(inv_get_handler_number("INV_DMP", &event_num) < 0) + printf("DMP not enabled\n"); + else + read_gesture(event_num); + break; + + case 3: + printf("-----------------\n"); + printf("--- pedometer ---\n"); + printf("-----------------\n"); + read_pedometer(sysfs_path); + break; + + default: + printf("error choice\n"); + break; + } + + gettimeofday(&tv, NULL); + clock_gettime(CLOCK_MONOTONIC, &tsp1); + clock_gettime(CLOCK_REALTIME, &tsp0); + + clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2); + clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3); + //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC, CLOCK_REALTIME, + // CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID); + //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec); + //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec); + //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec); + //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec); + + //ioctl (fd, EVIOCGNAME (sizeof (name)), name); + //printf ("Reading From : %s (%s)\n", argv[1], name); + + + return 0; +} + |