diff options
Diffstat (limited to 'libsensors_iio/software/simple_apps')
30 files changed, 4780 insertions, 2139 deletions
diff --git a/libsensors_iio/software/simple_apps/common/external_hardware.h b/libsensors_iio/software/simple_apps/common/external_hardware.h new file mode 100644 index 0000000..55e3b20 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/external_hardware.h @@ -0,0 +1,156 @@ +/* + Accelerometer +*/ +#define get_accel_slave_descr NULL + +#ifdef CONFIG_MPU_SENSORS_ADXL34X /* ADI accelerometer */ +struct ext_slave_descr *adxl34x_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr adxl34x_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_BMA150 /* Bosch accelerometer */ +struct ext_slave_descr *bma150_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr bma150_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_BMA222 /* Bosch 222 accelerometer */ +struct ext_slave_descr *bma222_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr bma222_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_BMA250 /* Bosch accelerometer */ +struct ext_slave_descr *bma250_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr bma250_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_KXSD9 /* Kionix accelerometer */ +struct ext_slave_descr *kxsd9_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr kxsd9_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_KXTF9 /* Kionix accelerometer */ +struct ext_slave_descr *kxtf9_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr kxtf9_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_LIS331DLH /* ST accelerometer */ +struct ext_slave_descr *lis331_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr lis331_get_slave_descr +#endif + + +#ifdef CONFIG_MPU_SENSORS_LIS3DH /* ST accelerometer */ +struct ext_slave_descr *lis3dh_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr lis3dh_get_slave_descr +#endif + +/* ST accelerometer in LSM303DLx combo */ +#if defined CONFIG_MPU_SENSORS_LSM303DLX_A +struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr lsm303dlx_a_get_slave_descr +#endif + +/* MPU6050 Accel */ +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 +struct ext_slave_descr *mpu6050_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr mpu6050_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_MMA8450 /* Freescale accelerometer */ +struct ext_slave_descr *mma8450_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr mma8450_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_MMA845X /* Freescale accelerometer */ +struct ext_slave_descr *mma845x_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr mma845x_get_slave_descr +#endif + + +/* + Compass +*/ +#define get_compass_slave_descr NULL + +#ifdef CONFIG_MPU_SENSORS_AK8975 /* AKM compass */ +struct ext_slave_descr *ak8975_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr ak8975_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_AMI30X /* AICHI Steel AMI304/305 compass */ +struct ext_slave_descr *ami30x_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr ami30x_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_AMI306 /* AICHI Steel AMI306 compass */ +struct ext_slave_descr *ami306_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr ami306_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_HMC5883 /* Honeywell compass */ +struct ext_slave_descr *hmc5883_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr hmc5883_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_MMC314X /* MEMSIC compass */ +struct ext_slave_descr *mmc314x_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr mmc314x_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_LSM303DLX_M /* ST compass */ +struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr lsm303dlx_m_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_YAS529 /* Yamaha compass */ +struct ext_slave_descr *yas529_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr yas529_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_YAS530 /* Yamaha compass */ +struct ext_slave_descr *yas530_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr yas530_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_HSCDTD002B /* Alps HSCDTD002B compass */ +struct ext_slave_descr *hscdtd002b_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr hscdtd002b_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_HSCDTD004A /* Alps HSCDTD004A compass */ +struct ext_slave_descr *hscdtd004a_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr hscdtd004a_get_slave_descr +#endif +/* + Pressure +*/ +#define get_pressure_slave_descr NULL + +#ifdef CONFIG_MPU_SENSORS_BMA085 /* BMA pressure */ +struct ext_slave_descr *bma085_get_slave_descr(void); +#undef get_pressure_slave_descr +#define get_pressure_slave_descr bma085_get_slave_descr +#endif diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.c b/libsensors_iio/software/simple_apps/common/fopenCMake.c new file mode 100644 index 0000000..2936109 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/fopenCMake.c @@ -0,0 +1,56 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/****************************************************************************** + * + * $Id: fopenCMake.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +#include <string.h> + +#include "fopenCMake.h" +#include "path_configure.h" + +/** + * @brief Replacement for fopen that concatenates the location of the + * source tree onto the filename path. + * It looks in 3 locations: + * - in the current directory, + * - then it looks in "..", + * - lastly in the define UNITTEST_SOURCE_DIR which + * gets defined by CMake. + * @param filename + * Filename relative to base of source directory. + * @param prop + * Second argument to fopen. + */ +FILE *fopenCMake(const char *filename, const char *prop) +{ + char path[150]; + FILE *file; + + // Look first in current directory + file = fopen(filename, prop); + if (file == NULL) { + // Now look in ".." +#ifdef WIN32 + strcpy(path, "..\\"); +#else + strcpy(path, "../"); +#endif + strcat(path, filename); + file = fopen(path, prop); + if (file == NULL) { + // Now look in definition by CMake + strcpy(path, PATH_SOURCE_DIR); + strcat(path, filename); + file = fopen(path, prop); + } + } + return file; +} + + diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.h b/libsensors_iio/software/simple_apps/common/fopenCMake.h new file mode 100644 index 0000000..c5eba39 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/fopenCMake.h @@ -0,0 +1,21 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef FOPEN_CMAKE_H__ +#define FOPEN_CMAKE_H__ + +#include <stdio.h> + +#ifdef __cplusplus +extern "C" { +#endif + +FILE *fopenCMake( const char *filename, const char *prop ); + +#ifdef __cplusplus +} +#endif + +#endif // FOPEN_CMAKE_H__ diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.c b/libsensors_iio/software/simple_apps/common/gestureMenu.c new file mode 100644 index 0000000..2a9487c --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/gestureMenu.c @@ -0,0 +1,725 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $ + *****************************************************************************/ + +#define _USE_MATH_DEFINES +#include <stdio.h> +#include <stddef.h> +#include <math.h> +#include <string.h> + +#include "ml.h" +#include "mlmath.h" +#include "gesture.h" +#include "orientation.h" +#include "gestureMenu.h" +#include "fifo.h" + +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "gest" +#include "log.h" +#include "mldl_cfg.h" + +static unsigned long sensors[] = { + INV_NINE_AXIS, + INV_THREE_AXIS_GYRO, + INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL, + INV_THREE_AXIS_ACCEL, + INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS, + INV_THREE_AXIS_COMPASS, + INV_SIX_AXIS_GYRO_ACCEL, + INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS, + INV_SIX_AXIS_ACCEL_COMPASS, +}; + +static char *sensors_string[] = { + "INV_NINE_AXIS", + "INV_THREE_AXIS_GYRO", + "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL", + "INV_THREE_AXIS_ACCEL", + "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS", + "INV_THREE_AXIS_COMPASS", + "INV_SIX_AXIS_GYRO_ACCEL", + "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS", + "INV_SIX_AXIS_ACCEL_COMPASS", +}; + +/** + * Prints the menu with the current thresholds + * + * @param params The parameters to print + */ +void PrintGestureMenu(tGestureMenuParams const * const params) +{ + MPL_LOGI("Press h at any time to re-display this menu\n"); + MPL_LOGI("TAP PARAMETERS:\n"); + MPL_LOGI(" Use LEFT and RIGHT arrows to adjust Tap Time \n\n"); + MPL_LOGI(" j : Increase X threshold : %5d\n", + params->xTapThreshold); + MPL_LOGI(" J (Shift-j): Decrease X threshold\n"); + MPL_LOGI(" k : Increase Y threshold : %5d\n", + params->yTapThreshold); + MPL_LOGI(" K (Shift-k): Decrease Y threshold\n"); + MPL_LOGI(" i : Increase Z threshold : %5d\n", + params->zTapThreshold); + MPL_LOGI(" I (Shift-i): Decrease Z threshold\n"); + MPL_LOGI(" l : Increase tap time : %5d\n", + params->tapTime); + MPL_LOGI(" L (Shift-l): Decrease tap time\n"); + MPL_LOGI(" o : Increase next tap time : %5d\n", + params->nextTapTime); + MPL_LOGI(" O (Shift-o): Increase next tap time\n"); + MPL_LOGI(" u : Increase max Taps : %5d\n", + params->maxTaps); + MPL_LOGI(" U (Shift-u): Increase max Taps\n"); + + MPL_LOGI("SHAKE PARAMETERS:\n"); + MPL_LOGI(" x : Increase X threshold : %5d\n", + params->xShakeThresh); + MPL_LOGI(" X (Shift-x): Decrease X threshold\n"); + MPL_LOGI(" y : Increase Y threshold : %5d\n", + params->yShakeThresh); + MPL_LOGI(" Y (Shift-y): Decrease Y threshold\n"); + MPL_LOGI(" z : Increase Z threshold : %5d\n", + params->zShakeThresh); + MPL_LOGI(" Z (Shift-z): Decrease Z threshold\n"); + MPL_LOGI(" s : Toggle Shake Function : %5d\n", + params->shakeFunction); + MPL_LOGI(" t : Increase Shake Time : %5d\n", + params->shakeTime); + MPL_LOGI(" T (Shift-T): Decrease Shake Time\n"); + MPL_LOGI(" n : Increase Next Shake Time : %5d\n", + params->nextShakeTime); + MPL_LOGI(" N (Shift-n): Decrease Next Shake Time\n"); + MPL_LOGI(" m : Increase max Shakes : %5d\n", + params->maxShakes); + MPL_LOGI(" M (Shift-m): Decrease max Shakes\n"); + MPL_LOGI("SNAP PARAMETERS:\n"); + MPL_LOGI(" p : Increase Pitch threshold : %5d\n", + params->xSnapThresh); + MPL_LOGI(" P (Shift-p): Decrease Pitch threshold\n"); + MPL_LOGI(" r : Increase Roll threshold : %5d\n", + params->ySnapThresh); + MPL_LOGI(" R (Shift-r): Decrease Roll threshold\n"); + MPL_LOGI(" a : Increase yAw threshold : %5d\n", + params->zSnapThresh); + MPL_LOGI(" A (Shift-a): Decrease yAw threshold\n"); + MPL_LOGI("YAW ROTATION PARAMETERS:\n"); + MPL_LOGI(" e : Increase yaW Rotate time : %5d\n", + params->yawRotateTime); + MPL_LOGI(" E (Shift-r): Decrease yaW Rotate time\n"); + MPL_LOGI(" w : Increase yaW Rotate threshold : %5d\n", + params->yawRotateThreshold); + MPL_LOGI(" W (Shift-w): Decrease yaW Rotate threshold\n"); + MPL_LOGI("ORIENTATION PARAMETER:\n"); + MPL_LOGI(" d : Increase orientation angle threshold : %5f\n", + params->orientationThreshold); + MPL_LOGI(" D (Shift-d): Decrease orientation angle threshold\n"); + MPL_LOGI("FIFO RATE:\n"); + MPL_LOGI(" f : Increase fifo divider : %5d\n", + inv_get_fifo_rate()); + MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); + MPL_LOGI("REQUESTED SENSORS:\n"); + MPL_LOGI(" S (Shift-s): Toggle in use sensors : %s\n", + sensors_string[params->sensorsIndex]); + MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); + + /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */ + /* S is available */ + + MPL_LOGI("\n\n"); +} + +/** + * Handles a keyboard input and updates an appropriate threshold, prints then + * menu or returns false if the character is not processed. + * + * @param params The parameters to modify if the thresholds are updated + * @param ch The input character + * + * @return true if the character was processed, false otherwise + */ +inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch) +{ + int result = INV_SUCCESS; + /* Dynamic keyboard processing */ + + switch (ch) { + case 'j': + params->xTapThreshold += 20; + // Intentionally fall through + case 'J': { + params->xTapThreshold -= 10; + if (params->xTapThreshold < 0) params->xTapThreshold = 0; + result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetTapThresh returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n", + params->xTapThreshold); + } break; + case 'k': + params->yTapThreshold += 20; + // Intentionally fall through + case 'K': { + params->yTapThreshold -= 10; + if (params->yTapThreshold < 0) params->yTapThreshold = 0; + result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetTapThresh returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n", + params->yTapThreshold); + } break; + case 'i': + params->zTapThreshold += 20; + // Intentionally fall through + case 'I': { + params->zTapThreshold -= 10; + if (params->zTapThreshold < 0) params->zTapThreshold = 0; + result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetTapThresh returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n", + params->zTapThreshold); + } break; + + case 'l': + params->tapTime += 20; + // Intentionally fall through + case 'L': { + params->tapTime -= 10; + if (params->tapTime < 0) params->tapTime = 0; + result = inv_set_next_tap_time(params->tapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); + } + MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime); + } break; + case 'o': + params->nextTapTime += 20; + // Intentionally fall through + case 'O': { + params->nextTapTime -= 10; + if (params->nextTapTime < 0) params->nextTapTime = 0; + result = MLSetNextTapTime(params->nextTapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetNextTapTime returned :%d\n", result); + } + MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime); + } break; + case 'u': + params->maxTaps += 2; + // Intentionally fall through + case 'U': { + params->maxTaps -= 1; + if (params->maxTaps < 0) params->maxTaps = 0; + result = inv_set_max_taps(params->maxTaps); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_taps returned :%d\n", result); + } + MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps); + } break; + case 's': { + int shakeParam; + params->shakeFunction = (params->shakeFunction + 1) % 2; + switch (params->shakeFunction) + { + case 0: + shakeParam = INV_NO_RETRACTION; + MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n"); + break; + case 1: + shakeParam = INV_RETRACTION; + MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n"); + break; + }; + result = inv_set_shake_func(shakeParam); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_func returned :%d\n", result); + } + } break; + case 'x': + params->xShakeThresh += 200; + // Intentionally fall through + case 'X': { + params->xShakeThresh -= 100; + result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh); + } break; + case 'y': + params->yShakeThresh += 200; + // Intentionally fall through + case 'Y': { + params->yShakeThresh -= 100; + result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh); + } break; + case 'z': + params->zShakeThresh += 200; + // Intentionally fall through + case 'Z':{ + params->zShakeThresh -= 100; + result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh); + } break; + case 'r': + params->ySnapThresh += 20; + // Intentionally fall through + case 'R': { + params->ySnapThresh -= 10; + result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh); + } break; + case 'p': + params->xSnapThresh += 20; + // Intentionally fall through + case 'P': { + params->xSnapThresh -= 10; + result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n", + params->xSnapThresh); + } break; + case 'a': + params->zSnapThresh += 20; + case 'A': { + params->zSnapThresh -= 10; + result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh); + } break; + + case 't': + params->shakeTime += 20; + case 'T':{ + params->shakeTime -= 10; + result = inv_set_shake_time(params->shakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_time returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime); + } break; + case 'n': + params->nextShakeTime += 20; + case 'N':{ + params->nextShakeTime -= 10; + result = inv_set_next_shake_time(params->nextShakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); + } + MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime); + } break; + case 'm': + params->maxShakes += 2; + case 'M':{ + params->maxShakes -= 1; + result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_shakes returned :%d\n", result); + } + MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes); + } break; + case 'e': + params->yawRotateTime += 20; + case 'E':{ + params->yawRotateTime -= 10; + result = inv_set_yaw_rotate_time(params->yawRotateTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); + } + MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime); + } break; + case 'w': + params->yawRotateThreshold += 2; + case 'W':{ + params->yawRotateThreshold -= 1; + result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold); + } break; + case 'c': + params->shakeRejectValue += 0.20f; + case 'C':{ + params->shakeRejectValue -= 0.10f; + result = inv_set_tap_shake_reject(params->shakeRejectValue); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue); + } break; + case 'd': + params->orientationThreshold += 10; + case 'D':{ + params->orientationThreshold -= 5; + if (params->orientationThreshold > 90) { + params->orientationThreshold = 90; + } + + if (params->orientationThreshold < 0 ) { + params->orientationThreshold = 0; + } + + result = inv_set_orientation_thresh(params->orientationThreshold, + 5, 80, + INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d," + " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n", + params->orientationThreshold, 5, 80); + } break; + case 'f': + result = inv_set_fifo_rate(inv_get_fifo_rate() + 1); + MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); + break; + case 'F': + { + unsigned short newRate = inv_get_fifo_rate(); + if (newRate > 0) + newRate--; + result = inv_set_fifo_rate(newRate); + MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); + break; + } + case 'S': + params->sensorsIndex++; + if (params->sensorsIndex >= ARRAY_SIZE(sensors)) { + params->sensorsIndex = 0; + } + result = inv_set_mpu_sensors( + sensors[params->sensorsIndex] & params->available_sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result, + sensors_string[params->sensorsIndex]); + break; + case 'h': + case 'H': { + PrintGestureMenu(params); + } break; + default: { + result = INV_ERROR; + } break; + }; + return result; +} + +/** + * Initializes the tGestureMenuParams to a set of defaults. + * + * @param params The parameters to initialize. + */ +void GestureMenuSetDefaults(tGestureMenuParams * const params) +{ + params->xTapThreshold = 100; + params->yTapThreshold = 100; + params->zTapThreshold = 100; + params->tapTime = 100; + params->nextTapTime = 600; + params->maxTaps = 2; + params->shakeRejectValue = 0.8f; + params->xShakeThresh = 750; + params->yShakeThresh = 750; + params->zShakeThresh = 750; + params->xSnapThresh = 160; + params->ySnapThresh = 320; + params->zSnapThresh = 160; + params->shakeTime = 100; + params->nextShakeTime = 1000; + params->shakeFunction = 0; + params->maxShakes = 3; + params->yawRotateTime = 80; + params->yawRotateThreshold = 70; + params->orientationThreshold = 60; + params->sensorsIndex = 0; + params->available_sensors = INV_NINE_AXIS; +} + +void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, + unsigned long available_sensors) +{ + params->available_sensors = available_sensors; +} +/** + * Call the appropriate MPL set threshold functions and checkes the error codes + * + * @param params The parametrs to use in setting the thresholds + * + * @return INV_SUCCESS or the first error code encountered. + */ +inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params) +{ + inv_error_t result = INV_SUCCESS; + + result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); + return result; + } + result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); + return result; + } + result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); + return result; + } + result = inv_set_next_tap_time(params->tapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); + return result; + } + result = MLSetNextTapTime(params->nextTapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetNextTapTime returned :%d\n", result); + return result; + } + result = inv_set_max_taps(params->maxTaps); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_taps returned :%d\n", result); + return result; + } + result = inv_set_tap_shake_reject(params->shakeRejectValue); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); + return result; + } + + //Set up shake gesture + result = inv_set_shake_func(params->shakeFunction); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_func returned :%d\n", result); + return result; + } + result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + return result; + } + result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + return result; + } + result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + return result; + } + result = inv_set_shake_time(params->shakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_time returned :%d\n", result); + return result; + } + result = inv_set_next_shake_time(params->nextShakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); + return result; + } + result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_shakes returned :%d\n", result); + return result; + } + + // Yaw rotate settings + result = inv_set_yaw_rotate_time(params->yawRotateTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); + return result; + } + result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); + return result; + } + + // Orientation settings + result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80, + INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result); + return result; + } + + // Requested Sensors + result = inv_set_mpu_sensors( + sensors[params->sensorsIndex] & params->available_sensors); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result, + sensors[params->sensorsIndex] & params->available_sensors); + return result; + } + + return INV_SUCCESS; +} + +void PrintGesture(tGesture* gesture) +{ + float speed; + char type[1024]; + switch (gesture->type) + { + case INV_TAP: + { + if (gesture->meta < 0) { + snprintf(type,sizeof(type),"-"); + } else { + snprintf(type,sizeof(type),"+"); + } + + switch (ABS(gesture->meta)) + { + case 1: + strcat(type,"X"); + break; + case 2: + strcat(type,"Y"); + break; + case 3: + strcat(type,"Z"); + break; + default: + strcat(type,"ERROR"); + break; + }; + MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n", + type, + gesture->num, + gesture->strength, + gesture->speed, + gesture->reserved, + (180 / M_PI) * atan2( + (float)gesture->strength, (float)gesture->speed), + (180 / M_PI) * atan2( + (float)gesture->speed, (float)gesture->reserved), + (180 / M_PI) * atan2( + (float)gesture->strength, (float)gesture->reserved) + ); + } + break; + case INV_ROLL_SHAKE: + case INV_PITCH_SHAKE: + case INV_YAW_SHAKE: + { + if (gesture->strength){ + snprintf(type, sizeof(type), "Snap : "); + } else { + snprintf(type, sizeof(type), "Shake: "); + } + + if (gesture->meta==0) { + strcat(type, "+"); + } else { + strcat(type, "-"); + } + + if (gesture->type == INV_ROLL_SHAKE) { + strcat(type, "Roll "); + } else if (gesture->type == INV_PITCH_SHAKE) { + strcat(type, "Pitch "); + } else if (gesture->type == INV_YAW_SHAKE) { + strcat(type, "Yaw "); + } + + speed = (float)gesture->speed + + (float)(gesture->reserved / (float)(1 << 16)); + MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed); + } + break; + case INV_YAW_IMAGE_ROTATE: + { + if (gesture->meta == 0) { + snprintf(type, sizeof(type), "Positive "); + } else { + snprintf(type, sizeof(type), "Negative "); + } + MPL_LOGI("%s Yaw Image Rotation\n", type); + } + break; + default: + MPL_LOGE("Unknown Gesture received\n"); + break; + } +} + +/** + * Prints the new or current orientation using MPL_LOGI and remembers the last + * orientation to print orientation flips. + * + * @param orientation the new or current orientation. 0 to reset. + */ +void PrintOrientation(unsigned short orientation) +{ + // Determine if it was a flip + static int sLastOrientation = 0; + int flip = orientation | sLastOrientation; + + if ((INV_X_UP | INV_X_DOWN) == flip) { + MPL_LOGI("Flip about the X Axis: \n"); + } else if ((INV_Y_UP | INV_Y_DOWN) == flip) { + MPL_LOGI("Flip about the Y axis: \n"); + } else if ((INV_Z_UP | INV_Z_DOWN) == flip) { + MPL_LOGI("Flip about the Z axis: \n"); + } + sLastOrientation = orientation; + + switch (orientation) { + case INV_X_UP: + MPL_LOGI("X Axis is up\n"); + break; + case INV_X_DOWN: + MPL_LOGI("X Axis is down\n"); + break; + case INV_Y_UP: + MPL_LOGI("Y Axis is up\n"); + break; + case INV_Y_DOWN: + MPL_LOGI("Y Axis is down\n"); + break; + case INV_Z_UP: + MPL_LOGI("Z Axis is up\n"); + break; + case INV_Z_DOWN: + MPL_LOGI("Z Axis is down\n"); + break; + case 0: + break; /* Not an error. Resets sLastOrientation */ + default: + MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation); + break; + } +} + + diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.h b/libsensors_iio/software/simple_apps/common/gestureMenu.h new file mode 100644 index 0000000..8f804e1 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/gestureMenu.h @@ -0,0 +1,75 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/***************************************************************************** * + * $Id: gestureMenu.h 5705 2011-06-28 19:32:29Z nroyer $ + ******************************************************************************/ +/** + * @defgroup + * @brief + * + * @{ + * @file gestureMenu.h + * @brief + * + * + */ + +#ifndef __GESTUREMENU_H__ +#define __GESTUREMENU_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************/ + typedef struct sGestureMenuParams { + /* Tap Params */ + int xTapThreshold; + int yTapThreshold; + int zTapThreshold; + int tapTime; + int nextTapTime; + int maxTaps; + float shakeRejectValue; + + /* Shake Params */ + int xShakeThresh; + int yShakeThresh; + int zShakeThresh; + int xSnapThresh; + int ySnapThresh; + int zSnapThresh; + int shakeTime; + int nextShakeTime; + int shakeFunction; + int maxShakes; + + /* Yaw rotate params */ + int yawRotateTime; + int yawRotateThreshold; + + /* Orientation */ + float orientationThreshold; + int sensorsIndex; + unsigned long available_sensors; + } tGestureMenuParams; + + void PrintGestureMenu(tGestureMenuParams const * const params) ; + inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params,char ch); + void PrintGesture(gesture_t* gesture); + void PrintOrientation(unsigned short orientation); + void GestureMenuSetDefaults(tGestureMenuParams * const params); + void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, + unsigned long available_sensors); + inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params); + +/******************************************************************************/ + +#ifdef __cplusplus +} +#endif + +#endif // __GESTUREMENU_H__ diff --git a/libsensors_iio/software/simple_apps/common/helper.c b/libsensors_iio/software/simple_apps/common/helper.c new file mode 100644 index 0000000..4d634bd --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/helper.c @@ -0,0 +1,110 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: helper.c 4367 2010-12-21 03:02:55Z prao $ + * + *******************************************************************************/ + +#include <stdio.h> +#ifdef _WIN32 +#include <windows.h> +#include <conio.h> +#endif +#ifdef LINUX +#include <sys/select.h> +#endif +#include <time.h> +#include <string.h> + +#include "ml.h" +#include "slave.h" +#include "mldl.h" +#include "mltypes.h" +#include "mlstates.h" +#include "compass.h" + +#include "mlsl.h" +#include "ml.h" + +#include "helper.h" +#include "mlsetup.h" +#include "fopenCMake.h" +#include "int.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-helper" + +#ifdef AIO +extern inv_error_t MLSLSetYamahaCompassDataMode(unsigned char mode); +#endif + +// Keyboard hit function +int ConsoleKbhit(void) +{ +#ifdef _WIN32 + return _kbhit(); +#else + 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; +#endif +} + +char ConsoleGetChar(void) { +#ifdef _WIN32 + return _getch(); +#else + return getchar(); +#endif +} +struct mpuirq_data** InterruptPoll(int *handles, int numHandles, long tv_sec, long tv_usec) +{ + struct mpuirq_data **data; + void *tmp; + int ii; + const int irq_data_size = sizeof(**data) * numHandles + + sizeof(*data) * numHandles; + + tmp = (void *)inv_malloc(irq_data_size); + memset(tmp, 0, irq_data_size); + data = (struct mpuirq_data **)tmp; + for (ii = 0; ii < numHandles; ii++) { + data[ii] = (struct mpuirq_data *)((unsigned long)tmp + + (sizeof(*data) * numHandles) + sizeof(**data) * ii); + } + + if (IntProcess(handles, numHandles, data, tv_sec, tv_usec) > 0) { + for (ii = 0; ii < numHandles; ii++) { + if (data[ii]->interruptcount) { + inv_interrupt_handler(ii); + } + } + } + + /* Return data incase the application needs to look at the timestamp or + other part of the data */ + return data; +} + +void InterruptPollDone(struct mpuirq_data ** data) +{ + inv_free(data); +} diff --git a/libsensors_iio/software/simple_apps/common/helper.h b/libsensors_iio/software/simple_apps/common/helper.h new file mode 100644 index 0000000..b2da520 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/helper.h @@ -0,0 +1,103 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: helper-customer.h 5770 2011-07-14 01:34:10Z mcaramello $ + * + *******************************************************************************/ + +#ifndef HELPER_C_H +#define HELPER_C_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "mlerrorcode.h" + +/* + Defines +*/ + +#define CALL_N_CHECK(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + } \ +} + +#define CALL_CHECK_N_RETURN_ERROR(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return r35uLt; \ + } \ +} + +// for functions returning void +#define CALL_CHECK_N_RETURN(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return; \ + } \ +} + +#define CALL_CHECK_N_EXIT(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + exit (r35uLt); \ + } \ +} + + +#define CALL_CHECK_N_CALLBACK(f, cb) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + cb; \ + } \ +} + +#define CALL_CHECK_N_GOTO(f, label) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + goto label; \ + } \ +} + +#define DEFAULT_PLATFORM PLATFORM_ID_MSB_V2 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 +#define DEFAULT_COMPASS_ID COMPASS_ID_AK8975 + +#define DataLogger(x) NULL +#define DataLoggerSelector(x) // +#define DataLoggerCb(x) NULL +#define findComm() (9) +#define MenuHwChoice(p,a,c) (*p = DEFAULT_PLATFORM, *a = DEFAULT_ACCEL_ID, \ + *c = DEFAULT_COMPASS_ID, INV_ERROR) + + char ConsoleGetChar(void); + int ConsoleKbhit(void); + struct mpuirq_data **InterruptPoll( + int *handles, int numHandles, long tv_sec, long tv_usec); + void InterruptPollDone(struct mpuirq_data ** data); + +#ifdef __cplusplus +} +#endif + +#endif // HELPER_C_H diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/libsensors_iio/software/simple_apps/common/mlerrorcode.c new file mode 100644 index 0000000..25b0df6 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.c @@ -0,0 +1,96 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +#include <stdio.h> +#include <string.h> + +#include "mltypes.h" +#include "mlerrorcode.h" + +#define ERROR_CODE_CASE(CODE) \ + case CODE: \ + return #CODE \ + +/** + * @brief return a string containing the label assigned to the error code. + * + * @param errorcode + * The errorcode value of which the label has to be returned. + * + * @return A string containing the error code label. + */ +char* MLErrorCode(inv_error_t errorcode) +{ + switch(errorcode) { + ERROR_CODE_CASE(INV_SUCCESS); + ERROR_CODE_CASE(INV_ERROR); + ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER); + ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED); + ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED); + ERROR_CODE_CASE(INV_ERROR_DMP_STARTED); + ERROR_CODE_CASE(INV_ERROR_NOT_OPENED); + ERROR_CODE_CASE(INV_ERROR_OPENED); + ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE); + ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED); + ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO); + ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE); + ERROR_CODE_CASE(INV_ERROR_FILE_OPEN); + ERROR_CODE_CASE(INV_ERROR_FILE_READ); + ERROR_CODE_CASE(INV_ERROR_FILE_WRITE); + + ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED); + ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR); + ERROR_CODE_CASE(INV_ERROR_SERIAL_READ); + ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE); + ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED); + + ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION); + ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE); + + ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW); + ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER); + ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT); + ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA); + ERROR_CODE_CASE(INV_ERROR_MEMORY_SET); + + ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR); + ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR); + + ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR); + ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE); + ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED); + ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED); + + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW); + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW); + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY); + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR); + + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD); + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE); + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN); + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM); + + default: + { + #define UNKNOWN_ERROR_CODE 1234 + return ERROR_NAME(UNKNOWN_ERROR_CODE); + break; + } + + } +} + +/** + * @} + */ diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/libsensors_iio/software/simple_apps/common/mlerrorcode.h new file mode 100644 index 0000000..9a35792 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.h @@ -0,0 +1,86 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $ + * + *******************************************************************************/ + +#ifndef _MLERRORCODE_H_ +#define _MLERRORCODE_H_ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Defines +*/ +#define CALL_N_CHECK(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + } \ +} + +#define CALL_CHECK_N_RETURN_ERROR(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return r35uLt; \ + } \ +} + +// for functions returning void +#define CALL_CHECK_N_RETURN(f) do { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return; \ + } \ + } while(0) + +#define CALL_CHECK_N_EXIT(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + exit (r35uLt); \ + } \ +} + + +#define CALL_CHECK_N_CALLBACK(f, cb) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + cb; \ + } \ +} + +#define CALL_CHECK_N_GOTO(f, label) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + goto label; \ + } \ +} + +char* MLErrorCode(inv_error_t errorcode); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c new file mode 100644 index 0000000..f11bce9 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlsetup.c @@ -0,0 +1,1722 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/****************************************************************************** + * + * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $ + * + *****************************************************************************/ +#undef MPL_LOG_NDEBUG +#ifdef UNITTESTING +#define MPL_LOG_NDEBUG 1 +#else +#define MPL_LOG_NDEBUG 0 +#endif + +/** + * @defgroup MLSETUP + * @brief The Motion Library external slaves setup override suite. + * + * Use these APIs to override the kernel/default settings in the + * corresponding data structures for gyros, accel, and compass. + * + * @{ + * @file mlsetup.c + * @brief The Motion Library external slaves setup override suite. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +/* + Defines +*/ +/* these have to appear before inclusion of mpu.h */ +#define CONFIG_MPU_SENSORS_KXSD9 y // Kionix accel +#define CONFIG_MPU_SENSORS_KXTF9 y // Kionix accel +#define CONFIG_MPU_SENSORS_LIS331DLH y // ST accelerometer +#define CONFIG_MPU_SENSORS_LSM303DLX_A y // ST accelerometer in LSM303DLx combo +#define CONFIG_MPU_SENSORS_LIS3DH y // ST accelerometer +#define CONFIG_MPU_SENSORS_BMA150 y // Bosch 150 accelerometer +#define CONFIG_MPU_SENSORS_BMA222 y // Bosch 222 accelerometer +#define CONFIG_MPU_SENSORS_BMA250 y // Bosch 250 accelerometer +#define CONFIG_MPU_SENSORS_ADXL34X y // AD 345 or 346 accelerometer +#define CONFIG_MPU_SENSORS_MMA8450 y // Freescale MMA8450 accelerometer +#define CONFIG_MPU_SENSORS_MMA845X y // Freescale MMA845X accelerometer +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 +#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y // Invensense MPU6050 built-in accelerometer +#endif + +#define CONFIG_MPU_SENSORS_AK8975 y // AKM compass +#define CONFIG_MPU_SENSORS_AMI30X y // AICHI AMI304/305 compass +#define CONFIG_MPU_SENSORS_AMI306 y // AICHI AMI306 compass +#define CONFIG_MPU_SENSORS_HMC5883 y // Honeywell compass +#define CONFIG_MPU_SENSORS_LSM303DLX_M y // ST compass in LSM303DLx combo +#define CONFIG_MPU_SENSORS_YAS529 y // Yamaha compass +#define CONFIG_MPU_SENSORS_YAS530 y // Yamaha compass +#define CONFIG_MPU_SENSORS_MMC314X y // MEMSIC compass +#define CONFIG_MPU_SENSORS_HSCDTD002B y // ALPS compass +#define CONFIG_MPU_SENSORS_HSCDTD004A y // ALPS HSCDTD004A compass + +#define CONFIG_MPU_SENSORS_BMA085 y // Bosch 085 pressure + +#include "external_hardware.h" + +#include <stdio.h> +#include <string.h> + +#include "slave.h" +#include "compass.h" +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mlsetup" + +#include "linux/mpu.h" + +#include "mlsetup.h" + +#ifdef LINUX +#include "errno.h" +#endif + +/* Override these structures from mldl.c */ +extern struct ext_slave_descr g_slave_accel; +extern struct ext_slave_descr g_slave_compass; +//extern struct ext_slave_descr g_slave_pressure; +/* Platform Data */ +//extern struct mpu_platform_data g_pdata; +extern struct ext_slave_platform_data g_pdata_slave_accel; +extern struct ext_slave_platform_data g_pdata_slave_compass; +//extern struct ext_slave_platform_data g_pdata_slave_pressure; +signed char g_gyro_orientation[9]; + +/* + Typedefs +*/ +typedef void tSetupFuncAccel(void); +typedef void tSetupFuncCompass(void); +typedef void tSetupFuncPressure(void); + +#ifdef LINUX +#include <sys/ioctl.h> +#endif + +/********************************************************************* + Dragon - PLATFORM_ID_DRAGON_PROTOTYPE +*********************************************************************/ +/** + * @internal + * @brief performs a 180' rotation around Z axis to reflect + * usage of the multi sensor board (MSB) with the + * beagleboard + * @note assumes well formed mounting matrix, with only + * one 1 for each row. + */ +static void Rotate180DegAroundZAxis(signed char matrix[]) +{ + int ii; + for(ii=0; ii<6; ii++) { + matrix[ii] = -matrix[ii]; + } +} + +/** + * @internal + * Sets the orientation based on the position of the mounting. For different + * devices the relative position to pin 1 will be different. + * + * Positions are: + * - 0-3 are Z up + * - 4-7 are Z down + * - 8-11 are Z right + * - 12-15 are Z left + * - 16-19 are Z front + * - 20-23 are Z back + * + * @param position The position of the orientation + * @param orientation the location to store the new oreintation + */ +static inv_error_t SetupOrientation(unsigned int position, + signed char *orientation) +{ + memset(orientation, 0, 9); + switch (position){ + case 0: + /*-------------------------*/ + orientation[0] = +1; + orientation[4] = +1; + orientation[8] = +1; + break; + case 1: + /*-------------------------*/ + orientation[1] = +1; + orientation[3] = -1; + orientation[8] = +1; + break; + case 2: + /*-------------------------*/ + orientation[0] = -1; + orientation[4] = -1; + orientation[8] = +1; + break; + case 3: + /*-------------------------*/ + orientation[1] = -1; + orientation[3] = +1; + orientation[8] = +1; + break; + case 4: + /*-------------------------*/ + orientation[0] = -1; + orientation[4] = +1; + orientation[8] = -1; + break; + case 5: + /*-------------------------*/ + orientation[1] = -1; + orientation[3] = -1; + orientation[8] = -1; + break; + case 6: + /*-------------------------*/ + orientation[0] = +1; + orientation[4] = -1; + orientation[8] = -1; + break; + case 7: + /*-------------------------*/ + orientation[1] = +1; + orientation[3] = +1; + orientation[8] = -1; + break; + case 8: + /*-------------------------*/ + orientation[2] = +1; + orientation[3] = +1; + orientation[7] = +1; + break; + case 9: + /*-------------------------*/ + orientation[2] = +1; + orientation[4] = +1; + orientation[6] = -1; + break; + case 10: + orientation[2] = +1; + orientation[3] = -1; + orientation[7] = -1; + break; + case 11: + orientation[2] = +1; + orientation[4] = -1; + orientation[6] = +1; + break; + case 12: + orientation[2] = -1; + orientation[3] = -1; + orientation[7] = +1; + break; + case 13: + orientation[2] = -1; + orientation[4] = -1; + orientation[6] = -1; + break; + case 14: + orientation[2] = -1; + orientation[3] = +1; + orientation[7] = -1; + break; + case 15: + orientation[2] = -1; + orientation[4] = +1; + orientation[6] = +1; + break; + case 16: + orientation[0] = -1; + orientation[5] = +1; + orientation[7] = +1; + break; + case 17: + orientation[1] = -1; + orientation[5] = +1; + orientation[6] = -1; + break; + case 18: + orientation[0] = +1; + orientation[5] = -1; + orientation[7] = -1; + break; + case 19: + orientation[1] = -1; + orientation[5] = +1; + orientation[6] = +1; + break; + case 20: + orientation[0] = +1; + orientation[5] = -1; + orientation[7] = +1; + break; + case 21: + orientation[1] = -1; + orientation[5] = -1; + orientation[6] = +1; + break; + case 22: + orientation[0] = -1; + orientation[5] = -1; + orientation[7] = -1; + break; + case 23: + orientation[1] = +1; + orientation[5] = -1; + orientation[6] = -1; + break; + default: + MPL_LOGE("Invalid position %d\n", position); + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return INV_SUCCESS; +} + +static void PrintMountingOrientation( + const char * header, signed char *orientation) +{ + MPL_LOGV("%s:\n", header); + MPL_LOGV("\t[[%3d, %3d, %3d]\n", + orientation[0], orientation[1], orientation[2]); + MPL_LOGV("\t [%3d, %3d, %3d]\n", + orientation[3], orientation[4], orientation[5]); + MPL_LOGV("\t [%3d, %3d, %3d]]\n", + orientation[6], orientation[7], orientation[8]); +} + +/***************************** + * Accel Setup Functions * + *****************************/ + +static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 5; + break; + case PLATFORM_ID_ST_6AXIS: + position = 0; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *lis331_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS331; + return INV_SUCCESS; +} + +static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 1; + break; + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 3; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *lis3dh_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS3DH; + return result; +} + +static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 7; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *kxsd9_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXSD9; + return result; +} + +static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB_EVB: + position =0; + break; + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 7; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + position = 1; + break; +#endif + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 1; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *kxtf9_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXTF9; + return result; +} + +static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + position = 3; + break; + case PLATFORM_ID_MSB_V2: + position = 1; + break; + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *lsm303dlx_a_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LSM303; + return result; +} + +static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + position = 3; + break; +#endif + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *bma150_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA150; + return result; +} + +static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 0; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *bma222_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA222; + return result; +} + +static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 0; + break; + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 3; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *bma250_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA250; + return result; +} + +static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *adxl34x_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_ADXL34X; + return result; +} + + +static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 5; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *mma8450_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA8450; + return result; +} + + +static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 5; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *mma845x_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA845X; + return result; +} + + +/** + * @internal + * Sets up the orientation matrix according to how the gyro was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MANTIS_MSB: + position = 6; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_DRAGON_USB_DONGLE: + position = 1; + break; + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + case PLATFORM_ID_MANTIS_EVB: + position = 0; + break; + case PLATFORM_ID_MSB_V3: + position = 2; + break; + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + return INV_ERROR_INVALID_PARAMETER; + }; + + SetupOrientation(position, g_pdata_slave_accel.orientation); + /* Interrupt */ +#ifndef LINUX +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 + // g_slave_accel = // fixme *mpu6050_get_slave_descr(); +#endif +#endif + g_pdata_slave_accel.address = 0x68; + return result; +} + +/***************************** + Compass Setup Functions +******************************/ +static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MANTIS_MSB: + position = 2; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_USB_DONGLE: + position = 4; + break; +#endif + case PLATFORM_ID_SPIDER_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + position = 7; + break; + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V3: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 6; + break; + case PLATFORM_ID_DRAGON_USB_DONGLE: + case PLATFORM_ID_MSB_EVB: + position = 5; + break; + case PLATFORM_ID_MANTIS_EVB: + position = 4; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *ak8975_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AKM; + return result; +} + +static inv_error_t SetupCompassMMCCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 7; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *mmc314x_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_MMC314X; + return result; +} + +static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 4; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI304; +#ifndef LINUX + g_slave_compass = *ami30x_get_slave_descr(); +#endif + return result; +} + +static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 3; + break; + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *ami306_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI306; + return result; +} + +static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + position = 2; + break; +#endif + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *hmc5883_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; + return result; +} + + +static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_10AXIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#ifndef LINUX + g_slave_compass = *lsm303dlx_m_get_slave_descr(); + g_slave_compass.id = COMPASS_ID_LSM303DLH; +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; + return result; +} + +static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + position = 8; + break; + case PLATFORM_ID_MSB_V2: + position = 12; + break; + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *lsm303dlx_m_get_slave_descr(); + g_slave_compass.id = COMPASS_ID_LSM303DLM; +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; + return result; +} + +static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *yas530_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS530; + return result; +} + +static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *yas529_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS529; + return result; +} + + +static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 2; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *hscdtd002b_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX; + return result; +} + +static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *hscdtd004a_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX; + return result; +} + + +/***************************** + Pressure Setup Functions +******************************/ +#if 0 +static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId) +{ + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation)); + + g_pdata_slave_pressure.bus = EXT_SLAVE_BUS_PRIMARY; +#ifndef LINUX + g_slave_pressure = *bma085_get_slave_descr(); +#endif + g_pdata_slave_pressure.address = PRESSURE_SLAVEADDR_BMA085; + return INV_SUCCESS; +} +#endif +/** + * @internal + * Sets up the orientation matrix according to how the part was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +static inv_error_t SetupAccelCalibration(unsigned short platformId, + unsigned short accelId) +{ + /*---- setup the accels ----*/ + switch(accelId) { + case ACCEL_ID_LSM303DLX: + SetupAccelLSM303Calibration(platformId); + break; + case ACCEL_ID_LIS331: + SetupAccelSTLIS331Calibration(platformId); + break; + case ACCEL_ID_KXSD9: + SetupAccelKionixKXSD9Calibration(platformId); + break; + case ACCEL_ID_KXTF9: + SetupAccelKionixKXTF9Calibration(platformId); + break; + case ACCEL_ID_BMA150: + SetupAccelBMA150Calibration(platformId); + break; + case ACCEL_ID_BMA222: + SetupAccelBMA222Calibration(platformId); + break; + case ACCEL_ID_BMA250: + SetupAccelBMA250Calibration(platformId); + break; + case ACCEL_ID_ADXL34X: + SetupAccelADXL34XCalibration(platformId); + break; + case ACCEL_ID_MMA8450: + SetupAccelMMA8450Calibration(platformId); + break; + case ACCEL_ID_MMA845X: + SetupAccelMMA845XCalibration(platformId); + break; + case ACCEL_ID_LIS3DH: + SetupAccelSTLIS3DHCalibration(platformId); + break; + case ACCEL_ID_MPU6050: + SetupAccelMPU6050Calibration(platformId); + break; + case ID_INVALID: + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) { + g_pdata_slave_accel.bus = EXT_SLAVE_BUS_SECONDARY; + } else if (accelId != ACCEL_ID_MPU6050) { + g_pdata_slave_accel.bus = EXT_SLAVE_BUS_PRIMARY; + } + +#ifndef WIN32 + if (accelId != ID_INVALID) + Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation); +#endif + + return INV_SUCCESS; +} + +/** + * @internal + * Sets up the orientation matrix according to how the part was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t SetupCompassCalibration(unsigned short platformId, + unsigned short compassId) +{ + /*---- setup the compass ----*/ + switch(compassId) { + case COMPASS_ID_AK8975: + SetupCompassAKM8975Calibration(platformId); + break; + case COMPASS_ID_AMI30X: + SetupCompassAMI304Calibration(platformId); + break; + case COMPASS_ID_AMI306: + SetupCompassAMI306Calibration(platformId); + break; + case COMPASS_ID_LSM303DLH: + SetupCompassLSM303DLHCalibration(platformId); + break; + case COMPASS_ID_LSM303DLM: + SetupCompassLSM303DLMCalibration(platformId); + break; + case COMPASS_ID_HMC5883: + SetupCompassHMC5883Calibration(platformId); + break; + case COMPASS_ID_YAS529: + SetupCompassYAS529Calibration(platformId); + break; + case COMPASS_ID_YAS530: + SetupCompassYAS530Calibration(platformId); + break; + case COMPASS_ID_MMC314X: + SetupCompassMMCCalibration(platformId); + break; + case COMPASS_ID_HSCDTD002B: + SetupCompassHSCDTD002BCalibration(platformId); + break; + case COMPASS_ID_HSCDTD004A: + SetupCompassHSCDTD004ACalibration(platformId); + break; + case ID_INVALID: + break; + default: + if (INV_ERROR_INVALID_PARAMETER) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + break; + } + + if (platformId == PLATFORM_ID_MSB_V2_MANTIS || + platformId == PLATFORM_ID_MANTIS_MSB || + platformId == PLATFORM_ID_MANTIS_USB_DONGLE || + platformId == PLATFORM_ID_MANTIS_PROTOTYPE || + platformId == PLATFORM_ID_DRAGON_PROTOTYPE) { + switch (compassId) { + case ID_INVALID: + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID; + break; + case COMPASS_ID_AK8975: + case COMPASS_ID_AMI306: + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_SECONDARY; + break; + default: + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY; + }; + } else { + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY; + } + +#ifndef WIN32 + if (compassId != ID_INVALID) + Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation); +#endif + + return INV_SUCCESS; +} + +/** + * @internal + * Sets up the orientation matrix according to how the part was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +#if 0 +inv_error_t SetupPressureCalibration(unsigned short platformId, + unsigned short pressureId) +{ + inv_error_t result = INV_SUCCESS; + /*---- setup the compass ----*/ + switch(pressureId) { + case PRESSURE_ID_BMA085: + result = SetupPressureBMA085Calibration(platformId); + break; + default: + if (INV_ERROR_INVALID_PARAMETER) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + }; + + return result; +} +#endif +/** + * @internal + * Sets up the orientation matrix according to how the gyro was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +static inv_error_t SetupGyroCalibration(unsigned short platformId) +{ + int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 2; + break; + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MANTIS_MSB: +#ifndef WIN32 + position = 4; +#else + position = 6; +#endif + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_USB_DONGLE: + position = 1; + break; + case PLATFORM_ID_DRAGON_USB_DONGLE: + position = 3; + break; + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: +#ifndef WIN32 + position = 2; +#else + position = 0; +#endif + break; + case PLATFORM_ID_MANTIS_EVB: + case PLATFORM_ID_MSB_EVB: + position = 0; + break; + case PLATFORM_ID_MSB_V3: + position = 2; + break; + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + return INV_ERROR_INVALID_PARAMETER; + }; + + SetupOrientation(position, g_gyro_orientation); + + return INV_SUCCESS; +} + +/** + * @brief Setup the Hw orientation and full scale. + * @param platfromId + * an user defined Id to distinguish the Hw platform in + * use from others. + * @param accelId + * the accelerometer specific id, as specified in the MPL. + * @param compassId + * the compass specific id, as specified in the MPL. + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t SetupPlatform( + unsigned short platformId, + unsigned short accelId, + unsigned short compassId) +{ + int result; + + memset(&g_slave_accel, 0, sizeof(g_slave_accel)); + memset(&g_slave_compass, 0, sizeof(g_slave_compass)); +// memset(&g_slave_pressure, 0, sizeof(g_slave_pressure)); +// memset(&g_pdata, 0, sizeof(g_pdata)); + +#ifdef LINUX + /* On Linux initialize the global platform data with the driver defaults */ + { + void *mpu_handle; + int ii; + + struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES]; + slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel; + slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass; + //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure; + + pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel; + pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass; + //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure; + + MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n"); + result = inv_serial_open("/dev/mpu",&mpu_handle); + if (result) { + MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result); + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!slave[ii]) + continue; + slave[ii]->type = ii; + result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR, + slave[ii]); + if (result) + result = errno; + if(result == INV_ERROR_INVALID_MODULE) { + slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE); + return result; + } + } + //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata); + if (result) { + result = errno; + LOG_RESULT_LOCATION(result); + return result; + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + continue; + pdata_slave[ii]->type = ii; + result = ioctl( + (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA, + pdata_slave[ii]); + if (result) + result = errno; + if (result == INV_ERROR_INVALID_MODULE) { + pdata_slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + if (result) { + MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result); + } + inv_serial_close(mpu_handle); + } +#endif + + result = SetupGyroCalibration(platformId); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Gyroscope", g_gyro_orientation); + result = SetupAccelCalibration(platformId, accelId); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation); + result = SetupCompassCalibration(platformId, compassId); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation); +#if 0 + if (platformId == PLATFORM_ID_MSB_10AXIS) { + result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation); + } +#endif +#ifdef LINUX + /* On Linux override the orientation, level shifter etc */ + { + void *mpu_handle; + int ii; + struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES]; + slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel; + slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass; + //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure; + + pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel; + pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass; + //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure; + + MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n"); + result = inv_serial_open("/dev/mpu",&mpu_handle); + if (result) { + MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result); + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!slave[ii]) + continue; + slave[ii]->type = ii; + result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA, + slave[ii]); + if (result) + result = errno; + if (result == INV_ERROR_INVALID_MODULE) { + slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata); + if (result) { + result = errno; + LOG_RESULT_LOCATION(result); + return result; + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + continue; + pdata_slave[ii]->type = ii; + result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA, + pdata_slave[ii]); + if (result) + result = errno; + if (result == INV_ERROR_INVALID_MODULE) { + pdata_slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + if (result) { + MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result); + } + inv_serial_close(mpu_handle); + } +#endif + return INV_SUCCESS; +} + +/** + * @} + */ + + diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.h b/libsensors_iio/software/simple_apps/common/mlsetup.h new file mode 100644 index 0000000..06fa9f4 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlsetup.h @@ -0,0 +1,52 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: mlsetup.h 6101 2011-09-29 00:30:33Z kkatingari $ + * + *******************************************************************************/ + +#ifndef MLSETUP_H +#define MLSETUP_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "linux/mpu.h" +#include "mltypes.h" + + enum mpu_platform_id { + PLATFORM_ID_INVALID = ID_INVALID, // 0 + PLATFORM_ID_MSB, // (0x0001) MSB (Multi sensors board) + PLATFORM_ID_ST_6AXIS, // (0x0002) 6 Axis with ST accelerometer + PLATFORM_ID_DONGLE, // (0x0003) 9 Axis USB dongle with + PLATFORM_ID_MANTIS_PROTOTYPE, // (0x0004) Mantis prototype board + PLATFORM_ID_MANTIS_MSB, // (0x0005) MSB with Mantis + PLATFORM_ID_MANTIS_USB_DONGLE,// (0x0006) Mantis and AKM on USB dongle. + PLATFORM_ID_MSB_10AXIS, // (0x0007) MSB with pressure sensor + PLATFORM_ID_DRAGON_PROTOTYPE, // (0x0008) Dragon prototype board + PLATFORM_ID_MSB_V2, // (0x0009) Version 2 MSB + PLATFORM_ID_MSB_V2_MANTIS, // (0x000A) Version 2 MSB with mantis + PLATFORM_ID_MANTIS_EVB, // (0x000B) Mantis EVB (shipped to cust.) + PLATFORM_ID_DRAGON_USB_DONGLE,// (0x000C) Dragon USB Dongle with Mantis Rev C + PLATFORM_ID_MSB_EVB, // (0X000D) MSB with 3050. + PLATFORM_ID_SPIDER_PROTOTYPE, + PLATFORM_ID_MSB_V3, + + NUM_PLATFORM_IDS + }; + // Main entry APIs +inv_error_t SetupPlatform(unsigned short platformId, + unsigned short accelSelection, + unsigned short compassSelection); + +#ifdef __cplusplus +} +#endif + +#endif /* MLSETUP_H */ diff --git a/libsensors_iio/software/simple_apps/common/slave.h b/libsensors_iio/software/simple_apps/common/slave.h new file mode 100644 index 0000000..7b40a8c --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/slave.h @@ -0,0 +1,176 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: slave.h 5732 2011-07-07 01:11:34Z vbhatt $ + * + *******************************************************************************/ + +#ifndef SLAVE_H +#define SLAVE_H + +/** + * @addtogroup SLAVEDL + * + * @{ + * @file slave.h + * @brief Top level descriptions for Accelerometer support + * + */ + +#include "mltypes.h" +#include "linux/mpu.h" + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +/*--- default accel support - selection ---*/ +#define ACCEL_ST_LIS331 0 +#define ACCEL_KIONIX_KXTF9 1 +#define ACCEL_BOSCH 0 +#define ACCEL_ADI 0 + +#define ACCEL_SLAVEADDR_INVALID 0x00 + +#define ACCEL_SLAVEADDR_LIS331 0x18 +#define ACCEL_SLAVEADDR_LSM303 0x18 +#define ACCEL_SLAVEADDR_LIS3DH 0x18 +#define ACCEL_SLAVEADDR_KXSD9 0x18 +#define ACCEL_SLAVEADDR_KXTF9 0x0F +#define ACCEL_SLAVEADDR_BMA150 0x38 +#define ACCEL_SLAVEADDR_BMA222 0x08 +#define ACCEL_SLAVEADDR_BMA250 0x18 +#define ACCEL_SLAVEADDR_ADXL34X 0x53 +#define ACCEL_SLAVEADDR_ADXL34X_ALT 0x1D /* alternative addr */ +#define ACCEL_SLAVEADDR_MMA8450 0x1C +#define ACCEL_SLAVEADDR_MMA845X 0x1C + +#define ACCEL_SLAVEADDR_INVENSENSE 0x68 +/* + Define default accelerometer to use if no selection is made +*/ +#if ACCEL_ST_LIS331 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LIS331 +#define DEFAULT_ACCEL_ID ACCEL_ID_LIS331 +#endif + +#if ACCEL_ST_LSM303 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LSM303 +#define DEFAULT_ACCEL_ID ACCEL_ID_LSM303DLX +#endif + +#if ACCEL_KIONIX_KXSD9 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXSD9 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXSD9 +#endif + +#if ACCEL_KIONIX_KXTF9 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXTF9 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 +#endif + +#if ACCEL_BOSCH +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA150 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA150 +#endif + +#if ACCEL_BMA222 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA222 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA222 +#endif + +#if ACCEL_BOSCH +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA250 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA250 +#endif + +#if ACCEL_ADI +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_ADXL34X +#define DEFAULT_ACCEL_ID ACCEL_ID_ADXL34X +#endif + +#if ACCEL_MMA8450 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA8450 +#define DEFAULT_ACCEL_ID ACCEL_ID_MMA8450 +#endif + +#if ACCEL_MMA845X +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA845X +#define DEFAULT_ACCEL_ID ACCEL_ID_MMA845X +#endif + +/*--- if no default accelerometer was selected ---*/ +#ifndef DEFAULT_ACCEL_SLAVEADDR +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_INVALID +#endif + +#define USE_COMPASS_AICHI 0 +#define USE_COMPASS_AKM 0 +#define USE_COMPASS_YAS529 0 +#define USE_COMPASS_YAS530 0 +#define USE_COMPASS_HMC5883 0 +#define USE_COMPASS_MMC314X 0 +#define USE_COMPASS_HSCDTD002B 0 +#define USE_COMPASS_HSCDTD004A 0 + +#define COMPASS_SLAVEADDR_INVALID 0x00 +#define COMPASS_SLAVEADDR_AKM_BASE 0x0C +#define COMPASS_SLAVEADDR_AKM 0x0E +#define COMPASS_SLAVEADDR_AMI304 0x0E +#define COMPASS_SLAVEADDR_AMI305 0x0F /*Slave address for AMI 305/306*/ +#define COMPASS_SLAVEADDR_AMI306 0x0E /*Slave address for AMI 305/306*/ +#define COMPASS_SLAVEADDR_YAS529 0x2E +#define COMPASS_SLAVEADDR_YAS530 0x2E +#define COMPASS_SLAVEADDR_HMC5883 0x1E +#define COMPASS_SLAVEADDR_MMC314X 0x30 +#define COMPASS_SLAVEADDR_HSCDTD00XX 0x0C + +/* + Define default compass to use if no selection is made +*/ + #if USE_COMPASS_AKM + #define DEFAULT_COMPASS_TYPE COMPASS_ID_AK8975 + #endif + + #if USE_COMPASS_AICHI + #define DEFAULT_COMPASS_TYPE COMPASS_ID_AMI30X + #endif + + #if USE_COMPASS_YAS529 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS529 + #endif + + #if USE_COMPASS_YAS530 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS530 + #endif + + #if USE_COMPASS_HMC5883 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_HMC5883 + #endif + +#if USE_COMPASS_MMC314X +#define DEFAULT_COMPASS_TYPE COMPASS_ID_MMC314X +#endif + +#if USE_COMPASS_HSCDTD002B +#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD002B +#endif + +#if USE_COMPASS_HSCDTD004A +#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD004A +#endif + +#ifndef DEFAULT_COMPASS_TYPE +#define DEFAULT_COMPASS_TYPE ID_INVALID +#endif + + +#endif // SLAVE_H + +/** + * @} + */ diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared Binary files differnew file mode 100644 index 0000000..228abf7 --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk index 7655e4d..b1d881c 100644 --- a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk +++ b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk @@ -1,4 +1,4 @@ -EXEC = inv_gesture_test$(SHARED_APP_SUFFIX) +EXEC = consoledmp$(SHARED_APP_SUFFIX) MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) @@ -8,10 +8,12 @@ LINK ?= $(CROSS)gcc OBJFOLDER = $(CURDIR)/obj -INV_ROOT = ../../../../.. +INV_ROOT = ../../../../../.. APP_DIR = $(CURDIR)/../.. MLLITE_DIR = $(INV_ROOT)/software/core/mllite +COMMON_DIR = $(INV_ROOT)/software/simple_apps/common MPL_DIR = $(INV_ROOT)/software/core/mpl +HAL_DIR = $(INV_ROOT)/software/core/HAL include $(INV_ROOT)/software/build/android/common.mk @@ -47,8 +49,20 @@ LLINK += -lstdc++ LLINK += -llog LLINK += -lz +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += $(ANDROID_LINK_EXECUTABLE) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib @@ -73,7 +87,7 @@ all: $(EXEC) $(MK_NAME) $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") - $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + $(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) $(OBJFOLDER) : @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") diff --git a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk new file mode 100644 index 0000000..b01fdfb --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk @@ -0,0 +1,23 @@ +#### filelist.mk for console_test #### + +# helper headers +HEADERS := $(COMMON_DIR)/external_hardware.h +HEADERS += $(COMMON_DIR)/fopenCMake.h +HEADERS += $(COMMON_DIR)/helper.h +HEADERS += $(COMMON_DIR)/mlerrorcode.h +HEADERS += $(COMMON_DIR)/mlsetup.h +HEADERS += $(COMMON_DIR)/slave.h + +HEADERS += $(HAL_DIR)/include/mlos.h +HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h + +# sources +SOURCES := $(APP_DIR)/console_test.c + +# helper sources +SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c +SOURCES += $(HAL_DIR)/linux/mlos_linux.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c new file mode 100644 index 0000000..e15b20d --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/console_test.c @@ -0,0 +1,742 @@ +/******************************************************************************* + * $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; +} diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared b/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared Binary files differdeleted file mode 100644 index ede049d..0000000 --- a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared +++ /dev/null diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk deleted file mode 100644 index 75d93cf..0000000 --- a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk +++ /dev/null @@ -1,11 +0,0 @@ -#### filelist.mk for inv_gesture_test #### - -# headers -#HEADERS += - -# sources -SOURCES := $(APP_DIR)/inv_gesture_test.c - -INV_SOURCES += $(SOURCES) - -VPATH += $(APP_DIR) diff --git a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c deleted file mode 100644 index d38d478..0000000 --- a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c +++ /dev/null @@ -1,535 +0,0 @@ -/** - * 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; -} - diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared Binary files differnew file mode 100644 index 0000000..5d52b21 --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk index cd79cfa..7f6cc43 100644 --- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk +++ b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk @@ -1,4 +1,4 @@ -EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX) +EXEC = input_gyro$(SHARED_APP_SUFFIX) MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) @@ -11,12 +11,13 @@ OBJFOLDER = $(CURDIR)/obj INV_ROOT = ../../../../.. APP_DIR = $(CURDIR)/../.. MLLITE_DIR = $(INV_ROOT)/software/core/mllite +COMMON_DIR = $(INV_ROOT)/software/simple_apps/common MPL_DIR = $(INV_ROOT)/software/core/mpl +HAL_DIR = $(INV_ROOT)/software/core/HAL include $(INV_ROOT)/software/build/android/common.mk CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) CFLAGS += -Wall CFLAGS += -fpic CFLAGS += -nostdlib @@ -48,8 +49,20 @@ LLINK += -lstdc++ LLINK += -llog LLINK += -lz +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += $(ANDROID_LINK_EXECUTABLE) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib @@ -74,7 +87,7 @@ all: $(EXEC) $(MK_NAME) $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") - $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + $(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) $(OBJFOLDER) : @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") diff --git a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk new file mode 100644 index 0000000..0936212 --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk @@ -0,0 +1,13 @@ +#### filelist.mk for input_gyro #### + +# helper headers +HEADERS := $(MPL_DIR)/authenticate.h +#HEADERS += + +# sources +SOURCES := $(APP_DIR)/test_input_gyro.c + +INV_SOURCES += $(SOURCES) + +#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux +VPATH += $(APP_DIR) 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; +} + diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared Binary files differdeleted file mode 100644 index 14ca523..0000000 --- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared +++ /dev/null diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk deleted file mode 100644 index 8a3977a..0000000 --- a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk +++ /dev/null @@ -1,12 +0,0 @@ -#### filelist.mk for mpu_iio #### - -# headers -#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h -HEADERS += $(APP_DIR)/iio_utils.h - -# sources -SOURCES := $(APP_DIR)/mpu_iio.c - -INV_SOURCES += $(SOURCES) - -VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h deleted file mode 100644 index 773ff2c..0000000 --- a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h +++ /dev/null @@ -1,643 +0,0 @@ -/* IIO - useful set of util functionality - * - * Copyright (c) 2008 Jonathan Cameron - * - * This program is free software; you can redistribute it and/or modify it - * under the terms of the GNU General Public License version 2 as published by - * the Free Software Foundation. - */ - -/* Made up value to limit allocation sizes */ -#include <string.h> -#include <stdlib.h> -#include <ctype.h> -#include <stdio.h> -#include <stdint.h> -#include <dirent.h> - -#define IIO_MAX_NAME_LENGTH 30 - -#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" -#define FORMAT_TYPE_FILE "%s_type" - -const char *iio_dir = "/sys/bus/iio/devices/"; - -/** - * iioutils_break_up_name() - extract generic name from full channel name - * @full_name: the full channel name - * @generic_name: the output generic channel name - **/ -static int iioutils_break_up_name(const char *full_name, - char **generic_name) -{ - char *current; - char *w, *r; - char *working; - current = strdup(full_name); - working = strtok(current, "_\0"); - w = working; - r = working; - - while (*r != '\0') { - if (!isdigit(*r)) { - *w = *r; - w++; - } - r++; - } - *w = '\0'; - *generic_name = strdup(working); - free(current); - - return 0; -} - -/** - * struct iio_channel_info - information about a given channel - * @name: channel name - * @generic_name: general name for channel type - * @scale: scale factor to be applied for conversion to si units - * @offset: offset to be applied for conversion to si units - * @index: the channel index in the buffer output - * @bytes: number of bytes occupied in buffer output - * @mask: a bit mask for the raw output - * @is_signed: is the raw value stored signed - * @enabled: is this channel enabled - **/ -struct iio_channel_info { - char *name; - char *generic_name; - float scale; - float offset; - unsigned index; - unsigned bytes; - unsigned bits_used; - unsigned shift; - uint64_t mask; - unsigned be; - unsigned is_signed; - unsigned enabled; - unsigned location; -}; - -/** - * iioutils_get_type() - find and process _type attribute data - * @is_signed: output whether channel is signed - * @bytes: output how many bytes the channel storage occupies - * @mask: output a bit mask for the raw data - * @be: big endian - * @device_dir: the iio device directory - * @name: the channel name - * @generic_name: the channel type name - **/ -inline int iioutils_get_type(unsigned *is_signed, - unsigned *bytes, - unsigned *bits_used, - unsigned *shift, - uint64_t *mask, - unsigned *be, - const char *device_dir, - const char *name, - const char *generic_name) -{ - FILE *sysfsfp; - int ret; - DIR *dp; - char *scan_el_dir, *builtname, *builtname_generic, *filename = 0; - char signchar, endianchar; - unsigned padint; - const struct dirent *ent; - - ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); - if (ret < 0) { - ret = -ENOMEM; - goto error_ret; - } - ret = asprintf(&builtname, FORMAT_TYPE_FILE, name); - if (ret < 0) { - ret = -ENOMEM; - goto error_free_scan_el_dir; - } - ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name); - if (ret < 0) { - ret = -ENOMEM; - goto error_free_builtname; - } - - dp = opendir(scan_el_dir); - if (dp == NULL) { - ret = -errno; - goto error_free_builtname_generic; - } - while (ent = readdir(dp), ent != NULL) - /* - * Do we allow devices to override a generic name with - * a specific one? - */ - if ((strcmp(builtname, ent->d_name) == 0) || - (strcmp(builtname_generic, ent->d_name) == 0)) { - ret = asprintf(&filename, - "%s/%s", scan_el_dir, ent->d_name); - if (ret < 0) { - ret = -ENOMEM; - goto error_closedir; - } - sysfsfp = fopen(filename, "r"); - if (sysfsfp == NULL) { - printf("failed to open %s\n", filename); - ret = -errno; - goto error_free_filename; - } - - ret = fscanf(sysfsfp, - "%ce:%c%u/%u>>%u", - &endianchar, - &signchar, - bits_used, - &padint, shift); - if (ret < 0) { - printf("failed to pass scan type description\n"); - return ret; - } - *be = (endianchar == 'b'); - *bytes = padint / 8; - if (*bits_used == 64) - *mask = ~0; - else - *mask = (1 << *bits_used) - 1; - if (signchar == 's') - *is_signed = 1; - else - *is_signed = 0; - fclose(sysfsfp); - free(filename); - - filename = 0; - } -error_free_filename: - if (filename) - free(filename); -error_closedir: - closedir(dp); -error_free_builtname_generic: - free(builtname_generic); -error_free_builtname: - free(builtname); -error_free_scan_el_dir: - free(scan_el_dir); -error_ret: - return ret; -} - -inline int iioutils_get_param_float(float *output, - const char *param_name, - const char *device_dir, - const char *name, - const char *generic_name) -{ - FILE *sysfsfp; - int ret; - DIR *dp; - char *builtname, *builtname_generic; - char *filename = NULL; - const struct dirent *ent; - - ret = asprintf(&builtname, "%s_%s", name, param_name); - if (ret < 0) { - ret = -ENOMEM; - goto error_ret; - } - ret = asprintf(&builtname_generic, - "%s_%s", generic_name, param_name); - if (ret < 0) { - ret = -ENOMEM; - goto error_free_builtname; - } - dp = opendir(device_dir); - if (dp == NULL) { - ret = -errno; - goto error_free_builtname_generic; - } - while (ent = readdir(dp), ent != NULL) - if ((strcmp(builtname, ent->d_name) == 0) || - (strcmp(builtname_generic, ent->d_name) == 0)) { - ret = asprintf(&filename, - "%s/%s", device_dir, ent->d_name); - if (ret < 0) { - ret = -ENOMEM; - goto error_closedir; - } - sysfsfp = fopen(filename, "r"); - if (!sysfsfp) { - ret = -errno; - goto error_free_filename; - } - fscanf(sysfsfp, "%f", output); - break; - } -error_free_filename: - if (filename) - free(filename); -error_closedir: - closedir(dp); -error_free_builtname_generic: - free(builtname_generic); -error_free_builtname: - free(builtname); -error_ret: - return ret; -} - -/** - * bsort_channel_array_by_index() - reorder so that the array is in index order - * - **/ - -inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array, - int cnt) -{ - - struct iio_channel_info temp; - int x, y; - - for (x = 0; x < cnt; x++) - for (y = 0; y < (cnt - 1); y++) - if ((*ci_array)[y].index > (*ci_array)[y+1].index) { - temp = (*ci_array)[y + 1]; - (*ci_array)[y + 1] = (*ci_array)[y]; - (*ci_array)[y] = temp; - } -} - -/** - * build_channel_array() - function to figure out what channels are present - * @device_dir: the IIO device directory in sysfs - * @ - **/ -inline int build_channel_array(const char *device_dir, - struct iio_channel_info **ci_array, - int *counter) -{ - DIR *dp; - FILE *sysfsfp; - int count, i; - struct iio_channel_info *current; - int ret; - const struct dirent *ent; - char *scan_el_dir; - char *filename; - - *counter = 0; - ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); - if (ret < 0) { - ret = -ENOMEM; - goto error_ret; - } - dp = opendir(scan_el_dir); - if (dp == NULL) { - ret = -errno; - goto error_free_name; - } - while (ent = readdir(dp), ent != NULL) - if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), - "_en") == 0) { - ret = asprintf(&filename, - "%s/%s", scan_el_dir, ent->d_name); - if (ret < 0) { - ret = -ENOMEM; - goto error_close_dir; - } - sysfsfp = fopen(filename, "r"); - if (sysfsfp == NULL) { - ret = -errno; - free(filename); - goto error_close_dir; - } - fscanf(sysfsfp, "%u", &ret); - printf("%s, %d\n", filename, ret); - if (ret == 1) - (*counter)++; - fclose(sysfsfp); - free(filename); - } - *ci_array = malloc(sizeof(**ci_array) * (*counter)); - if (*ci_array == NULL) { - ret = -ENOMEM; - goto error_close_dir; - } - closedir(dp); - dp = opendir(scan_el_dir); - //seekdir(dp, 0); - count = 0; - while (ent = readdir(dp), ent != NULL) { - if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), - "_en") == 0) { - current = &(*ci_array)[count++]; - ret = asprintf(&filename, - "%s/%s", scan_el_dir, ent->d_name); - if (ret < 0) { - ret = -ENOMEM; - /* decrement count to avoid freeing name */ - count--; - goto error_cleanup_array; - } - sysfsfp = fopen(filename, "r"); - if (sysfsfp == NULL) { - free(filename); - ret = -errno; - goto error_cleanup_array; - } - fscanf(sysfsfp, "%u", ¤t->enabled); - fclose(sysfsfp); - - if (!current->enabled) { - free(filename); - count--; - continue; - } - - current->scale = 1.0; - current->offset = 0; - current->name = strndup(ent->d_name, - strlen(ent->d_name) - - strlen("_en")); - if (current->name == NULL) { - free(filename); - ret = -ENOMEM; - goto error_cleanup_array; - } - /* Get the generic and specific name elements */ - ret = iioutils_break_up_name(current->name, - ¤t->generic_name); - if (ret) { - free(filename); - goto error_cleanup_array; - } - ret = asprintf(&filename, - "%s/%s_index", - scan_el_dir, - current->name); - if (ret < 0) { - free(filename); - ret = -ENOMEM; - goto error_cleanup_array; - } - sysfsfp = fopen(filename, "r"); - fscanf(sysfsfp, "%u", ¤t->index); - fclose(sysfsfp); - free(filename); - /* Find the scale */ - ret = iioutils_get_param_float(¤t->scale, - "scale", - device_dir, - current->name, - current->generic_name); - if (ret < 0) - goto error_cleanup_array; - ret = iioutils_get_param_float(¤t->offset, - "offset", - device_dir, - current->name, - current->generic_name); - if (ret < 0) - goto error_cleanup_array; - ret = iioutils_get_type(¤t->is_signed, - ¤t->bytes, - ¤t->bits_used, - ¤t->shift, - ¤t->mask, - ¤t->be, - device_dir, - current->name, - current->generic_name); - } - } - - closedir(dp); - /* reorder so that the array is in index order */ - bsort_channel_array_by_index(ci_array, *counter); - - return 0; - -error_cleanup_array: - for (i = count - 1; i >= 0; i--) - free((*ci_array)[i].name); - free(*ci_array); -error_close_dir: - closedir(dp); -error_free_name: - free(scan_el_dir); -error_ret: - return ret; -} - -inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify) -{ - int ret; - FILE *sysfsfp; - int test; - char *temp = malloc(strlen(basedir) + strlen(filename) + 2); - if (temp == NULL) - return -ENOMEM; - sprintf(temp, "%s/%s", basedir, filename); - sysfsfp = fopen(temp, "w"); - if (sysfsfp == NULL) { - printf("failed to open %s\n", temp); - ret = -errno; - goto error_free; - } - fprintf(sysfsfp, "%d", val); - fclose(sysfsfp); - if (verify) { - sysfsfp = fopen(temp, "r"); - if (sysfsfp == NULL) { - printf("failed to open %s\n", temp); - ret = -errno; - goto error_free; - } - fscanf(sysfsfp, "%d", &test); - if (test != val) { - printf("Possible failure in int write %d to %s%s\n", - val, - basedir, - filename); - ret = -1; - } - } -error_free: - free(temp); - return ret; -} - -int write_sysfs_int(char *filename, char *basedir, int val) -{ - return _write_sysfs_int(filename, basedir, val, 0); -} - -int write_sysfs_int_and_verify(char *filename, char *basedir, int val) -{ - return _write_sysfs_int(filename, basedir, val, 1); -} - -int _write_sysfs_string(char *filename, char *basedir, char *val, int verify) -{ - int ret = 0; - FILE *sysfsfp; - char *temp = malloc(strlen(basedir) + strlen(filename) + 2); - if (temp == NULL) { - printf("Memory allocation failed\n"); - return -ENOMEM; - } - sprintf(temp, "%s/%s", basedir, filename); - sysfsfp = fopen(temp, "w"); - if (sysfsfp == NULL) { - printf("Could not open %s\n", temp); - ret = -errno; - goto error_free; - } - fprintf(sysfsfp, "%s", val); - fclose(sysfsfp); - if (verify) { - sysfsfp = fopen(temp, "r"); - if (sysfsfp == NULL) { - printf("could not open file to verify\n"); - ret = -errno; - goto error_free; - } - fscanf(sysfsfp, "%s", temp); - if (strcmp(temp, val) != 0) { - printf("Possible failure in string write of %s " - "Should be %s " - "written to %s\%s\n", - temp, - val, - basedir, - filename); - ret = -1; - } - } -error_free: - free(temp); - - return ret; -} - -/** - * write_sysfs_string_and_verify() - string write, readback and verify - * @filename: name of file to write to - * @basedir: the sysfs directory in which the file is to be found - * @val: the string to write - **/ -int write_sysfs_string_and_verify(char *filename, char *basedir, char *val) -{ - return _write_sysfs_string(filename, basedir, val, 1); -} - -int write_sysfs_string(char *filename, char *basedir, char *val) -{ - return _write_sysfs_string(filename, basedir, val, 0); -} - -int read_sysfs_posint(char *filename, char *basedir) -{ - int ret; - FILE *sysfsfp; - char *temp = malloc(strlen(basedir) + strlen(filename) + 2); - if (temp == NULL) { - printf("Memory allocation failed"); - return -ENOMEM; - } - sprintf(temp, "%s/%s", basedir, filename); - sysfsfp = fopen(temp, "r"); - if (sysfsfp == NULL) { - ret = -errno; - goto error_free; - } - fscanf(sysfsfp, "%d\n", &ret); - fclose(sysfsfp); -error_free: - free(temp); - return ret; -} - -int read_sysfs_float(char *filename, char *basedir, float *val) -{ - float ret = 0; - FILE *sysfsfp; - char *temp = malloc(strlen(basedir) + strlen(filename) + 2); - if (temp == NULL) { - printf("Memory allocation failed"); - return -ENOMEM; - } - sprintf(temp, "%s/%s", basedir, filename); - sysfsfp = fopen(temp, "r"); - if (sysfsfp == NULL) { - ret = -errno; - goto error_free; - } - fscanf(sysfsfp, "%f\n", val); - fclose(sysfsfp); -error_free: - free(temp); - return ret; -} -int enable(const char *device_dir, - struct iio_channel_info **ci_array, - int *counter) -{ - DIR *dp; - int ret; - const struct dirent *ent; - char *scan_el_dir; - - *counter = 0; - ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); - if (ret < 0) { - ret = -ENOMEM; - goto error_ret; - } - dp = opendir(scan_el_dir); - if (dp == NULL) { - ret = -errno; - goto error_free_name; - } - while (ent = readdir(dp), ent != NULL) - if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), - "_en") == 0) { - write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 1); - } - return 0; -error_ret: -error_free_name: - return -1; -} -int disable_q_out(const char *device_dir, - struct iio_channel_info **ci_array, - int *counter) { - DIR *dp; - int ret; - const struct dirent *ent; - char *scan_el_dir; - - *counter = 0; - ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); - if (ret < 0) { - ret = -ENOMEM; - goto error_ret; - } - dp = opendir(scan_el_dir); - if (dp == NULL) { - ret = -errno; - goto error_free_name; - } - while (ent = readdir(dp), ent != NULL) - if (strncmp(ent->d_name, "in_quaternion", strlen("in_quaternion")) == 0) { - write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 0); - } - return 0; -error_ret: -error_free_name: - return -1; - -} - diff --git a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c deleted file mode 100644 index b3d323c..0000000 --- a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c +++ /dev/null @@ -1,685 +0,0 @@ -/* Industrialio buffer test code. - * - * Copyright (c) 2008 Jonathan Cameron - * - * This program is free software; you can redistribute it and/or modify it - * under the terms of the GNU General Public License version 2 as published by - * the Free Software Foundation. - * - * This program is primarily intended as an example application. - * Reads the current buffer setup from sysfs and starts a short capture - * from the specified device, pretty printing the result after appropriate - * conversion. - * - * Command line parameters - * generic_buffer -n <device_name> -t <trigger_name> - * If trigger name is not specified the program assumes you want a dataready - * trigger associated with the device and goes looking for it. - * - */ - -#include <unistd.h> -#include <dirent.h> -#include <fcntl.h> -#include <stdio.h> -#include <errno.h> -#include <sys/stat.h> -#include <dirent.h> -#include <linux/types.h> -#include <string.h> -#include <poll.h> -#include "iio_utils.h" -#include "ml_load_dmp.h" -#include "ml_sysfs_helper.h" -#include "authenticate.h" - -#define FLICK_SUPPORTED (0) - -/** - * size_from_channelarray() - calculate the storage size of a scan - * @channels: the channel info array - * @num_channels: size of the channel info array - * - * Has the side effect of filling the channels[i].location values used - * in processing the buffer output. - **/ -int size_from_channelarray(struct iio_channel_info *channels, int num_channels) -{ - int bytes = 0; - int i = 0; - while (i < num_channels) { - if (bytes % channels[i].bytes == 0) - channels[i].location = bytes; - else - channels[i].location = bytes - bytes%channels[i].bytes - + channels[i].bytes; - bytes = channels[i].location + channels[i].bytes; - i++; - } - return bytes; -} - -void print2byte(int input, struct iio_channel_info *info) -{ - /* shift before conversion to avoid sign extension - of left aligned data */ - input = input >> info->shift; - if (info->is_signed) { - int16_t val = input; - val &= (1 << info->bits_used) - 1; - val = (int16_t)(val << (16 - info->bits_used)) >> - (16 - info->bits_used); - /*printf("%d, %05f, scale=%05f", val, - (float)(val + info->offset)*info->scale, info->scale);*/ - printf("%d, ", val); - - } else { - uint16_t val = input; - val &= (1 << info->bits_used) - 1; - printf("%05f ", ((float)val + info->offset)*info->scale); - } -} -/** - * process_scan() - print out the values in SI units - * @data: pointer to the start of the scan - * @infoarray: information about the channels. Note - * size_from_channelarray must have been called first to fill the - * location offsets. - * @num_channels: the number of active channels - **/ -void process_scan(char *data, - struct iio_channel_info *infoarray, - int num_channels) -{ - int k; - //char *tmp; - for (k = 0; k < num_channels; k++) { - switch (infoarray[k].bytes) { - /* only a few cases implemented so far */ - case 2: - print2byte(*(uint16_t *)(data + infoarray[k].location), - &infoarray[k]); - //tmp = data + infoarray[k].location; - break; - case 4: - if (infoarray[k].is_signed) { - int32_t val = *(int32_t *) - (data + - infoarray[k].location); - if ((val >> infoarray[k].bits_used) & 1) - val = (val & infoarray[k].mask) | - ~infoarray[k].mask; - /* special case for timestamp */ - printf(" %d ", val); - } - break; - case 8: - if (infoarray[k].is_signed) { - int64_t val = *(int64_t *) - (data + - infoarray[k].location); - if ((val >> infoarray[k].bits_used) & 1) - val = (val & infoarray[k].mask) | - ~infoarray[k].mask; - /* special case for timestamp */ - if (infoarray[k].scale == 1.0f && - infoarray[k].offset == 0.0f) - printf(" %lld", val); - else - printf("%05f ", ((float)val + - infoarray[k].offset)* - infoarray[k].scale); - } - break; - default: - break; - } - } - printf("\n"); -} - -#if FLICK_SUPPORTED /* hide flick, not offially supported */ -void enable_flick(char *p, int on){ - int ret; - printf("flick:%s\n", p); - ret = write_sysfs_int_and_verify("flick_int_on", p, on); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("flick_upper", p, 3147790); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("flick_lower", p, -3147790); - if (ret < 0) - return; - - ret = write_sysfs_int_and_verify("flick_counter", p, 50); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("flick_message_on", p, 0); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("flick_axis", p, 0); -} -#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); -} -#define DMP_CODE_SIZE 3060 -void verify_img(char *dmp_path){ - FILE *fp; - int i; - char dmp_img[DMP_CODE_SIZE]; - if ((fp = fopen(dmp_path, "rb")) < 0 ) { - perror("dmp fail"); - } - i = fread(dmp_img, 1, DMP_CODE_SIZE, fp); - printf("Result=%d\n", i); - fclose(fp); - fp = fopen("/dev/read_img.h", "wt"); - fprintf(fp, "char rec[]={\n"); - for(i=0; i<DMP_CODE_SIZE; i++) { - fprintf(fp, "0x%02x, ", dmp_img[i]); - if(((i+1)%16) == 0) { - fprintf(fp, "\n"); - } - } - fprintf(fp, "};\n "); - fclose(fp); -} - -void setup_dmp(char *dev_path, int p_event){ - char sysfs_path[200]; - char dmp_path[200]; - int ret; - FILE *fd; - sprintf(sysfs_path, "%s", dev_path); - printf("sysfs: %s\n", sysfs_path); - ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1); - if (ret < 0) - return; - - ret = write_sysfs_int("in_accel_scale", dev_path, 0); - if (ret < 0) - return; - ret = write_sysfs_int("in_anglvel_scale", dev_path, 2); - if (ret < 0) - return; - ret = write_sysfs_int("sampling_frequency", sysfs_path, 200); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0); - if (ret < 0) - return; - sprintf(dmp_path, "%s/dmp_firmware", dev_path); - if ((fd = fopen(dmp_path, "wb")) < 0 ) { - perror("dmp fail"); - } - inv_load_dmp(fd); - fclose(fd); - printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path)); - ret = write_sysfs_int_and_verify("in_accel_x_offset", sysfs_path, 0xabcd0000); - ret = write_sysfs_int_and_verify("in_accel_y_offset", sysfs_path, 0xffff0000); - ret = write_sysfs_int_and_verify("in_accel_z_offset", sysfs_path, 0xcdef0000); - - ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1); - if (ret < 0) - return; - /* selelct which event to enable and interrupt on/off here */ - //enable_flick(sysfs_path, 1); - ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1); - if (ret < 0) - return; - printf("rate\n"); - ret = write_sysfs_int_and_verify("dmp_output_rate", sysfs_path, 25); - if (ret < 0) - return; - ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, p_event); - if (ret < 0) - return; - //verify_img(dmp_path); -} - -void get_dmp_event(char *dev_dir_name) -{ - char file_name[100]; - int i; -#if FLICK_SUPPORTED /* hide flick, not offially supported */ - int fp_tap, fp_orient, fp_disp, fp_flick; - const int n_gest = 6; -#else - int fp_tap, fp_orient, fp_disp, fp_motion; - //int fp_no_motion; - const int n_gest = 4; -#endif - int data; - char d[6]; - FILE *fp; - struct pollfd pfd[4]; - printf("%s\n", dev_dir_name); - while(1) { - sprintf(file_name, "%s/event_tap", dev_dir_name); - fp_tap = open(file_name, O_RDONLY | O_NONBLOCK); - sprintf(file_name, "%s/event_orientation", dev_dir_name); - fp_orient = open(file_name, O_RDONLY | O_NONBLOCK); - sprintf(file_name, "%s/event_display_orientation", dev_dir_name); - fp_disp = open(file_name, O_RDONLY | O_NONBLOCK); - - //sprintf(file_name, "%s/event_accel_motion", dev_dir_name); - sprintf(file_name, "%s/event_accel_wom", dev_dir_name); - fp_motion = open(file_name, O_RDONLY | O_NONBLOCK); - //sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name); - //fp_no_motion = open(file_name, O_RDONLY | O_NONBLOCK); -#if FLICK_SUPPORTED /* hide flick, not offially supported */ - sprintf(file_name, "%s/event_flick", dev_dir_name); - fp_flick = open(file_name, O_RDONLY | O_NONBLOCK); -#endif - - pfd[0].fd = fp_tap; - pfd[0].events = POLLPRI|POLLERR, - pfd[0].revents = 0; - - pfd[1].fd = fp_orient; - pfd[1].events = POLLPRI|POLLERR, - pfd[1].revents = 0; - - pfd[2].fd = fp_disp; - pfd[2].events = POLLPRI|POLLERR, - pfd[2].revents = 0; - - pfd[3].fd = fp_motion; - pfd[3].events = POLLPRI|POLLERR, - pfd[3].revents = 0; - - //pfd[4].fd = fp_no_motion; - //pfd[4].events = POLLPRI|POLLERR, - //pfd[4].revents = 0; - -#if FLICK_SUPPORTED /* hide flick, not offially supported */ - pfd[5].fd = fp_flick; - pfd[5].events = POLLPRI|POLLERR, - pfd[5].revents = 0; -#endif - - read(fp_tap, d, 4); - read(fp_orient, d, 4); - read(fp_disp, d, 4); - read(fp_motion, d, 4); - //read(fp_no_motion, d, 4); -#if FLICK_SUPPORTED /* hide flick, not offially supported */ - read(fp_flick, d, 4); -#endif - - poll(pfd, n_gest, -1); - close(fp_tap); - close(fp_orient); - close(fp_disp); - close(fp_motion); - //close(fp_no_motion); -#if FLICK_SUPPORTED /* hide flick, not offially supported */ - close(fp_flick); -#endif - for (i = 0; i < ARRAY_SIZE(pfd); i++) { - if(pfd[i].revents != 0) { - switch (i){ - case 0: - sprintf(file_name, "%s/event_tap", dev_dir_name); - fp = fopen(file_name, "rt"); - fscanf(fp, "%d\n", &data); - printf("tap=%x\n", data); - HandleTap(data); - fclose(fp); - break; - case 1: - sprintf(file_name, "%s/event_orientation", dev_dir_name); - fp = fopen(file_name, "rt"); - fscanf(fp, "%d\n", &data); - printf("orient=%x\n", data); - HandleOrient(data); - fclose(fp); - break; - case 2: - sprintf(file_name, "%s/event_display_orientation", dev_dir_name); - fp = fopen(file_name, "rt"); - fscanf(fp, "%d\n", &data); - printf("display_orient=%x\n", data); - fclose(fp); - break; - case 3: - sprintf(file_name, "%s/event_accel_wom", dev_dir_name); - fp = fopen(file_name, "rt"); - fscanf(fp, "%d\n", &data); - printf("motion=%x\n", data); - fclose(fp); - break; - case 4: - sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name); - fp = fopen(file_name, "rt"); - fscanf(fp, "%d\n", &data); - printf("No motion=%x\n", data); - fclose(fp); - break; - -#if FLICK_SUPPORTED /* hide flick, not offially supported */ - case 5: - sprintf(file_name, "%s/event_flick", dev_dir_name); - fp = fopen(file_name, "rt"); - fscanf(fp, "%d\n", &data); - printf("flick=%x\n", data); - fclose(fp); - break; -#endif - } - } - } - } -} - - -int main(int argc, char **argv) -{ - unsigned long num_loops = 2; - unsigned long timedelay = 100000; - unsigned long buf_len = 128; - - int ret, c, i, j, toread; - int fp; - - int num_channels; - char *trigger_name = NULL; - char *dev_dir_name, *buf_dir_name; - - int datardytrigger = 1; - char *data; - int read_size; - int dev_num, trig_num; - char *buffer_access; - int scan_size; - int noevents = 0; - int p_event = 0, nodmp = 0; - char *dummy; - char chip_name[10]; - char device_name[10]; - char sysfs[100]; - - struct iio_channel_info *infoarray; - /* -r means no DMP is enabled (raw) -> should be used for mpu3050. - -p means no print of data */ - /* when using -p, 1 means orientation, 2 means tap, 3 means flick */ - while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) { - switch (c) { - case 't': - trigger_name = optarg; - datardytrigger = 0; - break; - case 'e': - noevents = 1; - break; - case 'p': - p_event = 1; - break; - case 'r': - nodmp = 1; - break; - case 'c': - num_loops = strtoul(optarg, &dummy, 10); - break; - case 'w': - timedelay = strtoul(optarg, &dummy, 10); - break; - case 'l': - buf_len = strtoul(optarg, &dummy, 10); - break; - case '?': - return -1; - } - } - inv_get_sysfs_path(sysfs); - printf("sss:::%s\n", sysfs); - if (inv_get_chip_name(chip_name) != INV_SUCCESS) { - printf("get chip name fail\n"); - exit(0); - } - printf("chip_name=%s\n", chip_name); - if (INV_SUCCESS != inv_check_key()) - printf("key check fail\n"); - else - printf("key authenticated\n"); - - for (i=0; i<strlen(chip_name); i++) { - device_name[i] = tolower(chip_name[i]); - } - device_name[strlen(chip_name)] = '\0'; - printf("device name: %s\n", device_name); - - /* Find the device requested */ - dev_num = find_type_by_name(device_name, "iio:device"); - if (dev_num < 0) { - printf("Failed to find the %s\n", device_name); - ret = -ENODEV; - goto error_ret; - } - printf("iio device number being used is %d\n", dev_num); - asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num); - if (trigger_name == NULL) { - /* - * Build the trigger name. If it is device associated it's - * name is <device_name>_dev[n] where n matches the device - * number found above - */ - ret = asprintf(&trigger_name, - "%s-dev%d", device_name, dev_num); - if (ret < 0) { - ret = -ENOMEM; - goto error_ret; - } - } - ret = write_sysfs_int("buffer/enable", dev_dir_name, 0); - - ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1); -/* - ret = write_sysfs_int_and_verify("zero_motion_on", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("zero_motion_dur", dev_dir_name, 12); - ret = write_sysfs_int_and_verify("zero_motion_threshold", dev_dir_name, 13); - - ret = write_sysfs_int_and_verify("motion_on", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("motion_dur", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("motion_threshold", dev_dir_name, 1); -*/ - ret = write_sysfs_int_and_verify("accel_wom_on", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("accel_wom_threshold", dev_dir_name, 100); - /* Verify the trigger exists */ - trig_num = find_type_by_name(trigger_name, "trigger"); - if (trig_num < 0) { - printf("Failed to find the trigger %s\n", trigger_name); - ret = -ENODEV; - goto error_free_triggername; - } - printf("iio trigger number being used is %d\n", trig_num); - /* - * Parse the files in scan_elements to identify what channels are - * present - */ - ret = 0; - ret = enable(dev_dir_name, &infoarray, &num_channels); - if (ret) { - printf("error enable\n"); - goto error_free_triggername; - } - if (!nodmp) - setup_dmp(dev_dir_name, p_event); - - /* - * Construct the directory name for the associated buffer. - * As we know that the lis3l02dq has only one buffer this may - * be built rather than found. - */ - ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); - if (ret < 0) { - ret = -ENOMEM; - goto error_free_triggername; - } - printf("%s %s\n", dev_dir_name, trigger_name); - - /* Set the device trigger to be the data rdy trigger found above */ - ret = write_sysfs_string_and_verify("trigger/current_trigger", - dev_dir_name, - trigger_name); - if (ret < 0) { - printf("Failed to write current_trigger file\n"); - goto error_free_buf_dir_name; - } - /* Setup ring buffer parameters */ - /* length must be even number because iio_store_to_sw_ring is expecting - half pointer to be equal to the read pointer, which is impossible - when buflen is odd number. This is actually a bug in the code */ - ret = write_sysfs_int("length", buf_dir_name, buf_len*2); - if (ret < 0) - goto exit_here; - ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); - ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1); - //ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 0); - if (nodmp == 0) { - ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1); - } else { - ret = disable_q_out(dev_dir_name, &infoarray, &num_channels); - ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); - } - ret = build_channel_array(dev_dir_name, &infoarray, &num_channels); - if (ret) { - printf("Problem reading scan element information\n"); - goto exit_here; - } - - /* Enable the buffer */ - ret = write_sysfs_int("enable", buf_dir_name, 1); - if (ret < 0) - goto exit_here; - scan_size = size_from_channelarray(infoarray, num_channels); - data = malloc(scan_size*buf_len); - if (!data) { - ret = -ENOMEM; - goto exit_here; - } - - ret = asprintf(&buffer_access, - "/dev/iio:device%d", - dev_num); - if (ret < 0) { - ret = -ENOMEM; - goto error_free_data; - } - if (p_event) { - get_dmp_event(dev_dir_name); - goto error_free_buffer_access; - } - /* Attempt to open non blocking the access dev */ - fp = open(buffer_access, O_RDONLY | O_NONBLOCK); - if (fp == -1) { /*If it isn't there make the node */ - printf("Failed to open %s\n", buffer_access); - ret = -errno; - goto error_free_buffer_access; - } - /* Wait for events 10 times */ - for (j = 0; j < num_loops; j++) { - if (!noevents) { - struct pollfd pfd = { - .fd = fp, - .events = POLLIN, - }; - poll(&pfd, 1, -1); - toread = 1; - if ((j%128)==0) - usleep(timedelay); - - } else { - usleep(timedelay); - toread = 1; - } - read_size = read(fp, - data, - toread*scan_size); - if (read_size == -EAGAIN) { - printf("nothing available\n"); - continue; - } - if (0 == p_event) { - for (i = 0; i < read_size/scan_size; i++) - process_scan(data + scan_size*i, - infoarray, - num_channels); - } - } - close(fp); -error_free_buffer_access: - free(buffer_access); -error_free_data: - free(data); -exit_here: - /* Stop the ring buffer */ - ret = write_sysfs_int("enable", buf_dir_name, 0); - -error_free_buf_dir_name: - free(buf_dir_name); -error_free_triggername: - if (datardytrigger) - free(trigger_name); -error_ret: - return ret; -} diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared Binary files differindex 01d1425..33c9eef 100644 --- a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared +++ b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk index ed5fbf6..3a055cc 100644 --- a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk +++ b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk @@ -18,7 +18,6 @@ HAL_DIR = $(INV_ROOT)/software/core/HAL include $(INV_ROOT)/software/build/android/common.mk CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) CFLAGS += -Wall CFLAGS += -fpic CFLAGS += -nostdlib @@ -50,8 +49,20 @@ LLINK += -lstdc++ LLINK += -llog LLINK += -lz +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += $(ANDROID_LINK_EXECUTABLE) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib @@ -76,7 +87,7 @@ all: $(EXEC) $(MK_NAME) $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") - $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + $(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) $(OBJFOLDER) : @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") diff --git a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c index 2fe2cff..4f9996c 100644 --- a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c +++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c @@ -22,19 +22,13 @@ #include "ml_math_func.h" #include "storage_manager.h" #include "ml_stored_data.h" -#include "ml_sysfs_helper.h" #ifndef ABS #define ABS(x)(((x) >= 0) ? (x) : -(x)) #endif -//#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*)) - /** Change this key if the data being stored by this file changes */ -#define INV_DB_SAVE_KEY 53395 +#define INV_DB_SAVE_KEY 53394 #define FALSE 0 #define TRUE 1 @@ -44,29 +38,26 @@ #define COMPASS_PASS_STATUS_BIT 0x04 typedef union { - long l; + long l; int i; } bias_dtype; -char *sysfs_names_ptr; - -struct sysfs_attrbs { +struct inv_sysfs_names_s { char *enable; char *power_state; - char *dmp_on; - char *dmp_int_on; char *self_test; char *temperature; - char *gyro_enable; - char *gyro_x_bias; - char *gyro_y_bias; - char *gyro_z_bias; - char *accel_enable; - char *accel_x_bias; - char *accel_y_bias; - char *accel_z_bias; - char *compass_enable; -} mpu; + char *accl_bias; +}; + +const struct inv_sysfs_names_s mpu= { + /* MPU6050 & MPU9150 */ + .enable = "/sys/class/invensense/mpu/enable", + .power_state = "/sys/class/invensense/mpu/power_state", + .self_test = "/sys/class/invensense/mpu/self_test", + .temperature = "/sys/class/invensense/mpu/temperature", + .accl_bias = "/sys/class/invensense/mpu/accl_bias" +}; struct inv_db_save_t { /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ @@ -80,123 +71,46 @@ struct inv_db_save_t { /** Temperature when accel bias was stored. */ long accel_temp; long gyro_temp_slope[3]; - /** Sensor Accuracy */ - int gyro_accuracy; - int accel_accuracy; - int compass_accuracy; }; static struct inv_db_save_t save_data; -/** This function receives the data that was stored in non-volatile memory - between power off */ +/** This function receives the data that was stored in non-volatile memory between power off */ static inv_error_t inv_db_load_func(const unsigned char *data) { memcpy(&save_data, data, sizeof(save_data)); return INV_SUCCESS; } -/** This function returns the data to be stored in non-volatile memory between - power off */ +/** This function returns the data to be stored in non-volatile memory between power off */ static inv_error_t inv_db_save_func(unsigned char *data) { memcpy(data, &save_data, sizeof(save_data)); return INV_SUCCESS; } -/** read a sysfs entry that represents an integer */ -int read_sysfs_int(char *filename, int *var) +int inv_sysfs_write(char *filename, long data) { - int res=0; FILE *fp; + int count; - fp = fopen(filename, "r"); - if (fp != NULL) { - fscanf(fp, "%d\n", var); - fclose(fp); - } else { - MPL_LOGE("inv_self_test: ERR open file to read"); - res= -1; - } - return res; -} - -/** write a sysfs entry that represents an integer */ -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("inv_self_test: ERR open file to write"); - res= -1; - } - return res; -} - -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("inv_self_test: couldn't alloc mem for sysfs paths"); + if (!filename) 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.self_test, "%s%s", sysfs_path, "/self_test"); - sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); - - sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); - sprintf(mpu.gyro_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias"); - sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias"); - sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias"); - - sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); - sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias"); - sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias"); - sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias"); - - sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable"); - -#if 0 - // test print sysfs paths - dptr = (char**)&mpu; - for (i = 0; i < MAX_SYSFS_ATTRB; i++) { - MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++); - } -#endif - return 0; + fp = fopen(filename, "w"); + if (!fp) + return -errno; + count = fprintf(fp, "%ld", data); + fclose(fp); + return count; } -/******************************************************************************* - * M a i n S e l f T e s t - ******************************************************************************/ +/** + * Main Self test + */ int main(int argc, char **argv) { FILE *fptr; - int self_test_status = 0; + int self_test_status; inv_error_t result; bias_dtype gyro_bias[3]; bias_dtype accel_bias[3]; @@ -205,133 +119,76 @@ int main(int argc, char **argv) int axis_sign = 1; unsigned char *buffer; long timestamp; - int temperature = 0; - bool compass_present = TRUE; - - result = inv_init_sysfs_attributes(); - if (result) - return -1; + int temperature=0; + // Initialize storage manager inv_init_storage_manager(); // Clear out data. memset(&save_data, 0, sizeof(save_data)); - memset(gyro_bias, 0, sizeof(gyro_bias)); - memset(accel_bias, 0, sizeof(accel_bias)); + memset(gyro_bias,0, sizeof(gyro_bias)); + memset(accel_bias,0, sizeof(accel_bias)); // Register packet to be saved. - result = inv_register_load_store( - inv_db_load_func, inv_db_save_func, - sizeof(save_data), INV_DB_SAVE_KEY); + result = inv_register_load_store(inv_db_load_func, inv_db_save_func, + sizeof(save_data), + INV_DB_SAVE_KEY); // Power ON MPUxxxx chip - if (write_sysfs_int(mpu.power_state, 1) < 0) { - printf("Self-Test:ERR-Failed to set power state=1\n"); + if (inv_sysfs_write(mpu.power_state, 1) <0) { + printf("ERR- Failed to set power state=1\n"); } else { // Note: Driver turns on power automatically when self-test invoked } - // Disable Master enable - if (write_sysfs_int(mpu.enable, 0) < 0) { - printf("Self-Test:ERR-Failed to disable master enable\n"); - } - - // Disable DMP - if (write_sysfs_int(mpu.dmp_on, 0) < 0) { - printf("Self-Test:ERR-Failed to disable DMP\n"); - } - - // Enable Accel - if (write_sysfs_int(mpu.accel_enable, 1) < 0) { - printf("Self-Test:ERR-Failed to enable accel\n"); - } - - // Enable Gyro - if (write_sysfs_int(mpu.gyro_enable, 1) < 0) { - printf("Self-Test:ERR-Failed to enable gyro\n"); - } - - // Enable Compass - if (write_sysfs_int(mpu.compass_enable, 1) < 0) { -#ifdef DEBUG_PRINT - printf("Self-Test:ERR-Failed to enable compass\n"); -#endif - compass_present= FALSE; - } - fptr = fopen(mpu.self_test, "r"); - if (!fptr) { - printf("Self-Test:ERR-Couldn't invoke self-test\n"); - result = -1; - goto free_sysfs_storage; - } - - // Invoke self-test - fscanf(fptr, "%d", &self_test_status); - if (compass_present == TRUE) { - printf("Self-Test:Self test result- " - "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", - (self_test_status & GYRO_PASS_STATUS_BIT), - (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1, - (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2); - } else { - printf("Self-Test:Self test result- " - "Gyro passed= %x, Accel passed= %x\n", - (self_test_status & GYRO_PASS_STATUS_BIT), - (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1); - } - fclose(fptr); - - if (self_test_status & GYRO_PASS_STATUS_BIT) { - // Read Gyro Bias - if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 || - read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 || - read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) { - memset(gyro_bias, 0, sizeof(gyro_bias)); - printf("Self-Test:Failed to read Gyro bias\n"); - } else { - save_data.gyro_accuracy = 3; -#ifdef DEBUG_PRINT - printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n", - gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); -#endif + if (fptr != NULL) { + // Invoke self-test and read gyro bias + fscanf(fptr, "%d,%d,%d,%d", + &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status); + + printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", + (self_test_status & GYRO_PASS_STATUS_BIT), + (self_test_status & ACCEL_PASS_STATUS_BIT) >>1, + (self_test_status & COMPASS_PASS_STATUS_BIT) >>2); + printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); + fclose(fptr); + + if (!(self_test_status & GYRO_PASS_STATUS_BIT)) { + // Reset gyro bias data if gyro self-test failed + memset(gyro_bias,0, sizeof(gyro_bias)); + printf("Self-Test:Failed Gyro self-test\n"); } - } else { - printf("Self-Test:Failed Gyro self-test\n"); - } - if (self_test_status & ACCEL_PASS_STATUS_BIT) { - // Read Accel Bias - if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 || - read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 || - read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) { - memset(accel_bias,0, sizeof(accel_bias)); - printf("Self-Test:Failed to read Accel bias\n"); + if (self_test_status & ACCEL_PASS_STATUS_BIT) { + // Read Accel Bias + fptr= fopen(mpu.accl_bias, "r"); + if (fptr != NULL) { + fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i); + printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read accel bias\n"); + } } else { - save_data.accel_accuracy = 3; -#ifdef DEBUG_PRINT - printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n", - accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); -#endif - } - } else { - printf("Self-Test:Failed Accel self-test\n"); - } - - if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) { - printf("Self-Test:Failed Gyro and Accel self-test, " - "nothing left to do\n"); - result = -1; - goto free_sysfs_storage; - } + memset(accel_bias,0, sizeof(accel_bias)); + printf("Self-Test:Failed Accel self-test\n"); + } - // Read temperature - fptr= fopen(mpu.temperature, "r"); - if (fptr != NULL) { - fscanf(fptr,"%d %ld", &temperature, ×tamp); - fclose(fptr); + // Read temperature + if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT)) + { + fptr= fopen(mpu.temperature, "r"); + if (fptr != NULL) { + fscanf(fptr,"%d %ld", &temperature, ×tamp); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read temperature\n"); + } + } + } else { - printf("Self-Test:ERR-Couldn't read temperature\n"); + printf("Self-Test:ERR-Couldn't invoke self-test\n"); } // When we read gyro bias, the bias is in raw units scaled by 1000. @@ -368,16 +225,12 @@ int main(int argc, char **argv) save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f); #if 1 - printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", - save_data.accel_bias[0], save_data.accel_bias[1], - save_data.accel_bias[2]); - printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", - save_data.gyro_bias[0], save_data.gyro_bias[1], - save_data.gyro_bias[2]); - printf("Self-Test:Gyro temperature @ time stored %ld\n", - save_data.gyro_temp); - printf("Self-Test:Accel temperature @ time stored %ld\n", - save_data.accel_temp); + printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0], + save_data.accel_bias[1], save_data.accel_bias[2]); + printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0], + save_data.gyro_bias[1], save_data.gyro_bias[2]); + printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp); + printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp); #endif // Get size of packet to store. @@ -387,30 +240,25 @@ int main(int argc, char **argv) buffer = (unsigned char *)malloc(packet_sz + 10); if (buffer == NULL) { printf("Self-Test:Can't allocate buffer\n"); - result = -1; - goto free_sysfs_storage; + return -1; } - // Store the data result = inv_save_mpl_states(buffer, packet_sz); if (result) { - result = -1; + result= -1; } else { fptr= fopen(MLCAL_FILE, "wb+"); if (fptr != NULL) { fwrite(buffer, 1, packet_sz, fptr); fclose(fptr); } else { - printf("Self-Test:ERR- Can't open calibration file to write - %s\n", + printf("Self-Test:ERR- Can't open calibration file to write - %s\n", MLCAL_FILE); - result = -1; + result= -1; } } free(buffer); - -free_sysfs_storage: - free(sysfs_names_ptr); return result; } |