diff options
Diffstat (limited to 'libsensors_iio/software/simple_apps/common')
12 files changed, 3378 insertions, 0 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 + +/** + * @} + */ |