diff options
Diffstat (limited to 'libsensors_iio/software/simple_apps/console/linux/console_test.c')
| -rw-r--r-- | libsensors_iio/software/simple_apps/console/linux/console_test.c | 742 |
1 files changed, 0 insertions, 742 deletions
diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c deleted file mode 100644 index e15b20d..0000000 --- a/libsensors_iio/software/simple_apps/console/linux/console_test.c +++ /dev/null @@ -1,742 +0,0 @@ -/******************************************************************************* - * $Id: $ - ******************************************************************************/ - -/******************************************************************************* - * - * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved. - * - ******************************************************************************/ -//#include <linux/conio.h> -//#include <fcntl.h> -//#include <termios.h> -//#include <unistd.h> - -//#if 0 -#include <stdio.h> -#include <time.h> -#include <sys/select.h> -#include <unistd.h> -#include <string.h> -#include <sys/ioctl.h> -#include <fcntl.h> -#include <poll.h> -#include <stdlib.h> -#include <linux/input.h> - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "console_test" - -#include "mlos.h" -#include "invensense.h" -#include "invensense_adv.h" -#include "inv_sysfs_utils.h" -#include "ml_stored_data.h" -#include "ml_math_func.h" -#include "ml_load_dmp.h" -/*#else -#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 "inv_sysfs_utils.h" -#include "invensense.h" -#include "invensense_adv.h" -#include "ml_stored_data.h" -#include "ml_math_func.h" -#include "ml_load_dmp.h" -#include "log.h" -#endif*/ - -/* TODO: add devices as needed. */ -#define ITG3500 (0) -#define MPU6050 (1) -#define BMA250 (2) -#define NUM_DEVICES (ITG3500 + MPU6050 + BMA250) - -#define DEVICE MPU6050 -#define DMP_IMAGE dmp_firmware_200_latest -#define SIX_AXES 6 -#define NINE_AXES 9 - -#if 0 -struct input_event { - struct timeval time; - __u16 type; - __u16 code; - __s32 value; -}; -#endif - -/* TODO: Add paths to other attributes. - * TODO: Input device paths depend on the module installation order. - */ -const struct inv_sysfs_names_s filenames[NUM_DEVICES] = { - { /* ITG3500 */ - .buffer = "/dev/input/event0", - .enable = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable", - .raw_data = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro", - .temperature = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature", - .fifo_rate = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate", - .power_state = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state", - .fsr = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR", - .lpf = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf", - .scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale", - .temp_scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale", - .temp_offset = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset", - .self_test = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test", - .accel_en = NULL, - .accel_fifo_en = NULL, - .accel_fs = NULL, - .clock_source = NULL, - .early_suspend_en = NULL, - .firmware_loaded = NULL, - .gyro_en = NULL, - .gyro_fifo_en = NULL, - .key = NULL, - .raw_accel = NULL, - .reg_dump = NULL, - .tap_on = NULL, - .dmp_firmware = NULL - }, - - { /* MPU6050 */ - .buffer = "/dev/input/event0", - .enable = "/sys/class/invensense/mpu/enable", - .raw_data = "/sys/class/invensense/mpu/raw_gyro", - .temperature = "/sys/class/invensense/mpu/temperature", - .fifo_rate = "/sys/class/invensense/mpu/fifo_rate", - .power_state = "/sys/class/invensense/mpu/power_state", - .fsr = "/sys/class/invensense/mpu/FSR", - .lpf = "/sys/class/invensense/mpu/lpf", - .scale = "/sys/class/invensense/mpu/gyro_scale", - .temp_scale = "/sys/class/invensense/mpu/temp_scale", - .temp_offset = "/sys/class/invensense/mpu/temp_offset", - .self_test = "/sys/class/invensense/mpu/self_test", - .accel_en = "/sys/class/invensense/mpu/accl_enable", - .accel_fifo_en = "/sys/class/invensense/mpu/accl_fifo_enable", - .accel_fs = "/sys/class/invensense/mpu/accl_fs", - .clock_source = "/sys/class/invensense/mpu/clock_source", - .early_suspend_en = "/sys/class/invensense/mpu/early_suspend_enable", - .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded", - .gyro_en = "/sys/class/invensense/mpu/gyro_enable", - .gyro_fifo_en = "/sys/class/invensense/mpu/gyro_fifo_enable", - .key = "/sys/class/invensense/mpu/key", - .raw_accel = "/sys/class/invensense/mpu/raw_accl", - .reg_dump = "/sys/class/invensense/mpu/reg_dump", - .tap_on = "/sys/class/invensense/mpu/tap_on", - .dmp_firmware = "/sys/class/invensense/mpu/dmp_firmware" - }, - - { /* BMA250 */ - .buffer = "/dev/input/input/event1", - .enable = "/sys/devices/virtual/input/input1/enable", - .raw_data = "/sys/devices/virtual/input/input1/value", - .temperature = NULL, - .fifo_rate = NULL, - .power_state = NULL, - .fsr = NULL, - .lpf = NULL, - .scale = NULL, - .temp_scale = NULL, - .temp_offset = NULL, - .self_test = NULL, - .accel_en = NULL, - .accel_fifo_en = NULL, - .accel_fs = NULL, - .clock_source = NULL, - .early_suspend_en = NULL, - .firmware_loaded = NULL, - .gyro_en = NULL, - .gyro_fifo_en = NULL, - .key = NULL, - .raw_accel = NULL, - .reg_dump = NULL, - .tap_on = NULL, - .dmp_firmware = NULL - } -}; - -static void (*s_func_cb) (void); - -int inv_read_data(char *names, char *data) -{ - char str[8]; - int count; - short s_data; - - count = inv_sysfs_read((char*)names, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", &s_data); - *data = s_data; - if (count < 1) - return -EAGAIN; - return count; - -} - -void fifoCB(void) -{ - if (1) { - float gyro[3]; - float accel[3]; - float orient[3]; - float rv[3]; - - int8_t accuracy; - inv_time_t timestamp; - - printf("/*************************************************\n"); - inv_get_sensor_type_gyroscope(gyro, &accuracy, ×tamp); - printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n", - gyro[0], - gyro[1], - gyro[2], - accuracy, - timestamp); - - inv_get_sensor_type_accelerometer(accel, &accuracy, ×tamp); - printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n", - accel[0], - accel[1], - accel[2], - accuracy, - timestamp); - - inv_get_sensor_type_rotation_vector(rv, &accuracy, ×tamp); - printf("RV %7.3f %7.3f %7.3f %5d %9lld\n", - rv[0],rv[1],rv[2],accuracy,timestamp); - - inv_get_sensor_type_orientation(orient, &accuracy, ×tamp); - printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n", - orient[0],orient[1],orient[2],accuracy,timestamp); - printf("/*************************************************\n"); - } -} - -unsigned short orient; -signed char g_gyro_orientation[9] = {1, 0, 0, - 0, 1, 0, - 0, 0, 1}; - -signed char g_accel_orientation[9] = {-1, 0, 0, - 0, -1, 0, - 0, 0, 1}; -float scale; -float range; -long sens; - - - -short mTempOffset = 0; -short mTempScale = 0; -bool mFirstRead = 1; - -/******************* FUNCTIONS *******************************/ -#if 0 -static unsigned short inv_row_2_scale(const signed char *row) -{ - unsigned short b; - - if (row[0] > 0) - b = 0; - else if (row[0] < 0) - b = 4; - else if (row[1] > 0) - b = 1; - else if (row[1] < 0) - b = 5; - else if (row[2] > 0) - b = 2; - else if (row[2] < 0) - b = 6; - else - b = 7; // error - return b; -} -#endif - -inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)) -{ - s_func_cb = func_cb; - return INV_SUCCESS; -} - -/** - * @brief Keyboard hit function. - */ -int kbhit(void) -{ -#if 1 - struct timeval tv; - fd_set read_fd; - - tv.tv_sec=0; - tv.tv_usec=0; - FD_ZERO(&read_fd); - FD_SET(0,&read_fd); - - if (select(1, &read_fd, NULL, NULL, &tv) == -1) - return 0; - - if (FD_ISSET(0,&read_fd)) - return 1; - - return 0; -#else - struct timeval tv; - fd_set rdfs; - - tv.tv_sec = 0; - tv.tv_usec = 0; - - FD_ZERO(&rdfs); - FD_SET (STDIN_FILENO, &rdfs); - - select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv); - return FD_ISSET(STDIN_FILENO, &rdfs); -#endif -} - -inv_error_t inv_constructor_default_enable() -{ - inv_error_t result; - - result = inv_enable_quaternion(); - if (result) { - if (result == INV_ERROR_NOT_AUTHORIZED) { - LOGE("Enable Quaternion failed: not authorized"); - } - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_motion_no_motion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_gyro_tc(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_hal_outputs(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_9x_sensor_fusion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -int read_attribute_sensor(int fd, char *data, unsigned int size) -{ - int count = 0; - if (fd >=0) { - count = read(fd, data, size); - if(count < 0) { - MPL_LOGE("read fails with error code=%d", count); - } - close(fd); - } - return count; -} - -int inv_read_temperature(long long *data) -{ - int count = 0; - int fd; - - if(mFirstRead) { - char buf[4]; - fd = open(filenames[ITG3500].temp_scale, O_RDONLY); - if(fd < 0) { - MPL_LOGE("errors opening tempscale"); - return -1; - } - - memset(buf, 0, sizeof(buf)); - - count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 0) { - MPL_LOGE("errors reading temp_scale"); - return -1; - } - - count = sscanf(buf, "%hd", &mTempScale); - if(count < 1) - return -1; - MPL_LOGI("temp scale = %d", mTempScale); - - fd = open(filenames[ITG3500].temp_offset, O_RDONLY); - if(fd < 0) { - MPL_LOGE("errors opening tempoffset"); - return -1; - } - - memset(buf, 0, sizeof(buf)); - - count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 0) { - MPL_LOGE("errors reading temp_offset"); - return -1; - } - - count = sscanf(buf, "%hd", &mTempOffset); - if(count < 1) - return -1; - MPL_LOGI("temp offset = %d", mTempOffset); - - mFirstRead = false; - } - - char raw_buf[25]; - short raw; - long long timestamp; - fd = open(filenames[ITG3500].temperature, O_RDONLY); - if(fd < 0) { - MPL_LOGE("errors opening temperature"); - return -1; - } - - memset(raw_buf, 0, sizeof(raw_buf)); - - count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); - if(count < 0) { - MPL_LOGE("errors reading temperature"); - return -1; - } - count = sscanf(raw_buf, "%hd%lld", &raw, ×tamp); - if(count < -1) - return -1; - MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp); - MPL_LOGI("temperature offset = %d", mTempOffset); - MPL_LOGI("temperature scale = %d", mTempScale); - int adjuster = 35 + ((raw-mTempOffset)/mTempScale); - MPL_LOGI("pre-scaled temperature = %d", adjuster); - MPL_LOGI("adjusted temperature = %d", adjuster*65536); - //data[0] = adjuster * 65536; - data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f; - data[1] = timestamp; - return 0; -} - -int self_test(void) -{ - int err = 0; - char str[50]; - char x[9], y[9], z[9]; - char pass[2]; - int fd; - - fd = open((char*)filenames[DEVICE].self_test, O_RDONLY); - if(fd < 0) { - return fd; - } - memset(str, 0, sizeof(str)); - err = read_attribute_sensor(fd, str, sizeof(str)); - if(err < 0) { - return err; - } - MPL_LOGI("self_test result: %s", str); - printf("Self test result: %s ", str); - err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass); - if(err < 1) { - return err; - } - MPL_LOGI("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); - //printf("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); - if (atoi(pass)) { - MPL_LOGI("Result : PASS (1)"); - printf("----> PASS (1)\n"); - } else { - MPL_LOGI("Result : FAIL (0)"); - printf("----> FAIL (0)\n"); - } - return err; -} - -/******************************************************************************* - ******************************* MAIN ****************************************** - ******************************************************************************/ - -/** - * @brief Main function - */ -int main(int argc, char *argv[]) -{ - int key = 0; - int ready; - long accel[3]; - short gyro[3]; - long long timestamp = 0; - inv_error_t result; - - char data; - unsigned char i; - int fd, bytes_read; - struct pollfd pfd; - unsigned long long time_stamp; - unsigned int time_H; - struct input_event ev[100]; -#ifdef INV_PLAYBACK_DBG - int logging = false; - FILE *logfile = NULL; -#endif - - result = inv_init_mpl(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - // Master Enabling. This also turns on power_state - if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0) - printf("ERR- Failed to enable event generation\n"); - else { - inv_read_data((char *)filenames[DEVICE].enable, &data); - printf("Event enable= %d\n", data); - } - - // Power ON - No need after master enable above but do it anyway - if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0) - printf("ERR- Failed to set power state=1\n"); - else { - inv_read_data((char *)filenames[DEVICE].power_state, &data); - printf("Power state: %d\n", data); - } - - // Turn on tap - if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) { - printf("ERR- Failed to enable Tap On\n"); - } - else { - inv_read_data((char *)filenames[DEVICE].tap_on, &data); - printf("Tap-on: %d\n", data); - } - - // Program DMP code. No longer required to enable tap-on first - if ((result = - inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) { - printf("ERR- Failed to initiate DMP re-programming %d\n",result); - } else { - if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) { - printf("ERR- Failed file open to write DMP\n"); - close(fd); - exit(0); - } else { - // Program 200Hz version - //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE)); - //printf("Downloaded %d byte(s) to DMP\n", result); - result = inv_load_dmp(fd); - //LOG_RESULT_LOCATION(result); - close(fd); - } - } - - // Query DMP running. For now check by 'firmware_loaded' status - if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) { - printf("ERR- Failed to read 'firmware_loaded'\n"); - } else { - printf("Firmware Loaded/ DMP running: %d\n", data); - } - - inv_set_fifo_processed_callback(fifoCB); - result = inv_constructor_default_enable(); - result = inv_start_mpl(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else { - printf ("MPL started\n"); - } - - /* Gyro Setup */ - orient = inv_orientation_matrix_to_scalar(g_gyro_orientation); - inv_set_gyro_orientation_and_scale(orient,2000L<<15); - - /* Accel Setup */ - orient = inv_orientation_matrix_to_scalar(g_accel_orientation); - /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data. - * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits. - * 2 * 1L<<15 * 1<<6 == 1LL<<22 - */ - inv_set_accel_orientation_and_scale(orient, 1LL<<22); - - // Enable Gyro - if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0) - printf("ERR- Failed to enable Gyro\n"); - else { - inv_read_data((char *)filenames[DEVICE].gyro_en, &data); - printf("Gyro enable: %d\n", data); - } - - // Enable Accel - if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0) - printf("ERR- Failed to enable Accel\n"); - else { - inv_read_data((char *)filenames[DEVICE].accel_en, &data); - printf("Accel enable: %d\n", data); - } - - // polling for data - fd = open(filenames[DEVICE].buffer, O_RDONLY); - if(fd < 0) { - MPL_LOGE("Cannot open device event buffer"); - } - - pfd.fd = fd; - pfd.events = POLLIN; - - while (1) { - - result = kbhit(); - if (result) { - key = getchar(); - } else { - key = 0; - } - if (key == 'l') { - MPL_LOGI(" 'l' - load calibration file"); - inv_load_calibration(); - } - if (key == 't') { - MPL_LOGI(" 't' - self test"); - self_test(); - } - if (key == 'q') { - MPL_LOGI(" 'q' - store calibration file"); - inv_store_calibration(); - break; - } -#ifdef INV_PLAYBACK_DBG - if (key == 's') { - if (!logging) { - MPL_LOGI(" 's' - toggle logging on"); - logfile = fopen("/data/playback.bin", "wb"); - if (logfile) { - inv_turn_on_data_logging(logfile); - logging = true; - } else { - MPL_LOGI("Error : " - "cannot open log file '/data/playback.bin'"); - } - } else { - MPL_LOGI(" 's' - toggle logging off"); - inv_turn_off_data_logging(); - fclose(logfile); - logging = false; - } - break; - } -#endif - - ready = poll(&pfd, 1, 100); - if (ready) { - bytes_read = read_attribute_sensor(fd, (char *)ev, - sizeof(struct input_event) * SIX_AXES); - //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES); - if (bytes_read > 0) { - int executed; - - for (i = 0; i < bytes_read / sizeof(struct input_event); i++) { - if (ev[i].type == EV_REL) { - switch (ev[i].code) { - case REL_X: - printf("REL_X\n"); - gyro[0]= ev[i].value; //Gyro X - printf("Gyro X:%5d ", gyro[0]); - break; - case REL_Y: - printf("REL_Y\n"); - gyro[1]= ev[i].value; //Gyro Y - printf("Gyro Y:%5d ", gyro[1]); - break; - case REL_Z: - printf("REL_Z\n"); - gyro[2]= ev[i].value; //Gyro Z - printf("Gyro Z:%5d ", gyro[2]); - break; - case REL_RX: - printf("REL_RX\n"); - accel[0]= ev[i].value; //Accel X - printf("Accl X:%5ld ", accel[0]); - break; - case REL_RY: - printf("REL_RY\n"); - accel[1]= ev[i].value; //Accel Y - printf("Accl Y:%5ld ", accel[1]); - break; - case REL_RZ: - printf("REL_RZ\n"); - accel[2]= ev[i].value; //Accel Z - printf("Accl Z:%5ld ", accel[2]); - break; - case REL_MISC: - time_H= ev[i].value; - break; - case REL_WHEEL: - time_stamp = ((unsigned long long)(time_H) << 32) + - (unsigned int)ev[i].value; - break; - default: - printf("ERR- Un-recognized event code: %5d ", ev[i].code); - break; - } - } else { -#if 0 - clock_gettime(CLOCK_MONOTONIC, &timer); - curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL; - printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL); -#endif - } - } - - // build & process gyro + accel data - result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed); - if (result) { - LOG_RESULT_LOCATION(result); - } else if ((result = inv_build_accel(accel, 0, - (inv_time_t)timestamp, - &executed))) { - LOG_RESULT_LOCATION(result); - } - if (executed) { - printf("Exec on data Ok\n"); - s_func_cb(); - } - - } else { - //printf ("ERR- No data!\n"); - } - - } else { MPL_LOGV("Device not ready"); } - } - close(fd); - -#ifdef INV_PLAYBACK_DBG - if (logging) { - inv_turn_off_data_logging(); - fclose(logfile); - } -#endif - - return 0; -} |