HACK: libsensors: Initial attempt at MotionApps 5.1 for MPU6050 + AKM8975

The code in this patch still needs work.
But checking it in will allow other parts of the sensor stack to have
something to work with.

1. Have sensor HAL for IIO driver, which uses MPU6050 + AKM8975 (on 2nd bus).
2. Include MPL binaries (libmllite.so/libmplmpu.so).
3. Include necessary header files.
4. remove light sensor dependency.
5. add missing include file.
6. modify the module name into "manta".
7. remove mlsdk directory.
8. Fix some known issues.
9. Sync up to June.12.
10. Tweak slightly so that it can be used with other sensors on manta and
  doesn't break other devices that need a non IIO libsensors.

Change-Id: I0913a6df56fb0e99e9bae9ecc40ab03884d68124
Signed-off-by: Mars Lee <mlee@invensense.com>

24 files changed, 5243 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
+
+/**
+ *  @}
+ */
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
new file mode 100644
index 0000000..228abf7
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
new file mode 100644
index 0000000..b1d881c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = consoledmp$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
new file mode 100644
index 0000000..b01fdfb
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
@@ -0,0 +1,23 @@
+#### filelist.mk for console_test ####
+
+# helper headers
+HEADERS := $(COMMON_DIR)/external_hardware.h
+HEADERS += $(COMMON_DIR)/fopenCMake.h
+HEADERS += $(COMMON_DIR)/helper.h
+HEADERS += $(COMMON_DIR)/mlerrorcode.h
+HEADERS += $(COMMON_DIR)/mlsetup.h
+HEADERS += $(COMMON_DIR)/slave.h
+
+HEADERS += $(HAL_DIR)/include/mlos.h
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/console_test.c
+
+# helper sources
+SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c
+SOURCES += $(HAL_DIR)/linux/mlos_linux.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c
new file mode 100644
index 0000000..e15b20d
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/console_test.c
@@ -0,0 +1,742 @@
+/*******************************************************************************
+ * $Id: $
+ ******************************************************************************/
+ 
+/*******************************************************************************
+ *
+ * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved.
+ *
+ ******************************************************************************/
+//#include <linux/conio.h>
+//#include <fcntl.h>
+//#include <termios.h>
+//#include <unistd.h>
+
+//#if 0
+#include <stdio.h>
+#include <time.h>
+#include <sys/select.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <linux/input.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "console_test"
+
+#include "mlos.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "inv_sysfs_utils.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+/*#else
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+#include "inv_sysfs_utils.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+#include "log.h"
+#endif*/
+
+/* TODO: add devices as needed. */
+#define ITG3500         (0)
+#define MPU6050         (1)
+#define BMA250          (2)
+#define NUM_DEVICES     (ITG3500 + MPU6050 + BMA250)
+
+#define DEVICE          MPU6050
+#define DMP_IMAGE       dmp_firmware_200_latest
+#define SIX_AXES        6
+#define NINE_AXES       9
+
+#if 0
+struct input_event {
+       struct timeval time;
+       __u16 type;
+       __u16 code;
+       __s32 value;
+};
+#endif
+
+/* TODO: Add paths to other attributes.
+ * TODO: Input device paths depend on the module installation order.
+ */
+const struct inv_sysfs_names_s filenames[NUM_DEVICES] = {
+    {        /* ITG3500 */
+        .buffer             = "/dev/input/event0",
+        .enable             = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable",
+        .raw_data           = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro",
+        .temperature        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature",
+        .fifo_rate          = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate",
+        .power_state        = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state",
+        .fsr                = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR",
+        .lpf                = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf",
+        .scale              = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale",
+        .temp_scale         = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale",
+        .temp_offset        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset",
+        .self_test          = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test",
+        .accel_en           = NULL,
+        .accel_fifo_en      = NULL,
+        .accel_fs           = NULL,
+        .clock_source       = NULL,
+        .early_suspend_en   = NULL,
+        .firmware_loaded    = NULL,
+        .gyro_en            = NULL,
+        .gyro_fifo_en       = NULL,
+        .key                = NULL,
+        .raw_accel          = NULL,
+        .reg_dump           = NULL,
+        .tap_on             = NULL,
+        .dmp_firmware       = NULL
+    },
+
+    {        /* MPU6050 */
+        .buffer             = "/dev/input/event0",
+        .enable             = "/sys/class/invensense/mpu/enable",
+        .raw_data           = "/sys/class/invensense/mpu/raw_gyro",
+        .temperature        = "/sys/class/invensense/mpu/temperature",
+        .fifo_rate          = "/sys/class/invensense/mpu/fifo_rate",
+        .power_state        = "/sys/class/invensense/mpu/power_state",
+        .fsr                = "/sys/class/invensense/mpu/FSR",
+        .lpf                = "/sys/class/invensense/mpu/lpf",
+        .scale              = "/sys/class/invensense/mpu/gyro_scale",
+        .temp_scale         = "/sys/class/invensense/mpu/temp_scale",
+        .temp_offset        = "/sys/class/invensense/mpu/temp_offset",
+        .self_test          = "/sys/class/invensense/mpu/self_test",
+        .accel_en           = "/sys/class/invensense/mpu/accl_enable",
+        .accel_fifo_en      = "/sys/class/invensense/mpu/accl_fifo_enable",
+        .accel_fs           = "/sys/class/invensense/mpu/accl_fs",
+        .clock_source       = "/sys/class/invensense/mpu/clock_source",
+        .early_suspend_en   = "/sys/class/invensense/mpu/early_suspend_enable",
+           .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded",
+        .gyro_en            = "/sys/class/invensense/mpu/gyro_enable",
+        .gyro_fifo_en       = "/sys/class/invensense/mpu/gyro_fifo_enable",
+        .key                = "/sys/class/invensense/mpu/key",
+        .raw_accel          = "/sys/class/invensense/mpu/raw_accl",
+        .reg_dump           = "/sys/class/invensense/mpu/reg_dump",
+        .tap_on             = "/sys/class/invensense/mpu/tap_on",
+         .dmp_firmware      = "/sys/class/invensense/mpu/dmp_firmware"
+    },
+
+    {        /* BMA250 */
+        .buffer             = "/dev/input/input/event1",
+        .enable             = "/sys/devices/virtual/input/input1/enable",
+        .raw_data           = "/sys/devices/virtual/input/input1/value",
+        .temperature        = NULL,
+        .fifo_rate          = NULL,
+        .power_state        = NULL,
+        .fsr                = NULL,
+        .lpf                = NULL,
+        .scale              = NULL,
+        .temp_scale         = NULL,
+        .temp_offset        = NULL,
+        .self_test          = NULL,
+        .accel_en           = NULL,
+        .accel_fifo_en      = NULL,
+        .accel_fs           = NULL,
+        .clock_source       = NULL,
+        .early_suspend_en   = NULL,
+        .firmware_loaded    = NULL,
+        .gyro_en            = NULL,
+        .gyro_fifo_en       = NULL,
+        .key                = NULL,
+        .raw_accel          = NULL,
+        .reg_dump           = NULL,
+        .tap_on             = NULL,
+        .dmp_firmware       = NULL
+    }
+};
+
+static void (*s_func_cb) (void);
+
+int inv_read_data(char *names, char *data)
+{
+    char str[8];
+    int count;
+    short s_data;
+
+    count = inv_sysfs_read((char*)names, sizeof(str), str);
+    if (count < 0)
+        return count;
+    count = sscanf(str, "%hd", &s_data);
+    *data = s_data;
+    if (count < 1)
+        return -EAGAIN;
+    return count;
+
+}
+
+void fifoCB(void)
+{
+    if (1) {
+        float gyro[3];
+        float accel[3];
+        float orient[3];
+        float rv[3];
+
+        int8_t accuracy;
+        inv_time_t timestamp;
+
+        printf("/*************************************************\n"); 
+        inv_get_sensor_type_gyroscope(gyro, &accuracy, &timestamp);
+        printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n", 
+                     gyro[0],
+                     gyro[1],
+                     gyro[2],
+                     accuracy,
+                     timestamp);
+
+        inv_get_sensor_type_accelerometer(accel, &accuracy, &timestamp);
+        printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n", 
+                     accel[0],
+                     accel[1],
+                     accel[2],
+                     accuracy,
+                     timestamp);
+
+        inv_get_sensor_type_rotation_vector(rv, &accuracy, &timestamp);
+        printf("RV %7.3f %7.3f %7.3f %5d %9lld\n",
+                     rv[0],rv[1],rv[2],accuracy,timestamp);
+
+        inv_get_sensor_type_orientation(orient, &accuracy, &timestamp);
+        printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n",
+                     orient[0],orient[1],orient[2],accuracy,timestamp);
+        printf("/*************************************************\n"); 
+    }
+}
+
+unsigned short orient;
+signed char g_gyro_orientation[9] = {1, 0, 0,
+                                     0, 1, 0,
+                                     0, 0, 1};
+
+signed char g_accel_orientation[9] = {-1, 0, 0,
+                                      0, -1, 0,
+                                      0, 0, 1};
+float scale;
+float range;
+long sens;
+
+
+
+short mTempOffset = 0;
+short mTempScale = 0;
+bool mFirstRead = 1;
+
+/******************* FUNCTIONS *******************************/
+#if 0
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+    unsigned short b;
+
+    if (row[0] > 0)
+        b = 0;
+    else if (row[0] < 0)
+        b = 4;
+    else if (row[1] > 0)
+        b = 1;
+    else if (row[1] < 0)
+        b = 5;
+    else if (row[2] > 0)
+        b = 2;
+    else if (row[2] < 0)
+        b = 6;
+    else
+        b = 7;                  // error
+    return b;
+}
+#endif 
+
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
+{
+    s_func_cb = func_cb;
+    return INV_SUCCESS;
+}
+
+/**
+ * @brief Keyboard hit function.
+ */
+int kbhit(void)
+{
+#if 1
+    struct timeval tv;
+    fd_set read_fd;
+
+    tv.tv_sec=0;
+    tv.tv_usec=0;
+    FD_ZERO(&read_fd);
+    FD_SET(0,&read_fd);
+
+    if (select(1, &read_fd, NULL, NULL, &tv) == -1)
+        return 0;
+
+    if (FD_ISSET(0,&read_fd))
+        return 1;
+
+    return 0;
+#else
+    struct timeval tv;  
+    fd_set rdfs;   
+    
+    tv.tv_sec = 0;  
+    tv.tv_usec = 0;   
+    
+    FD_ZERO(&rdfs);  
+    FD_SET (STDIN_FILENO, &rdfs);   
+    
+    select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);  
+    return FD_ISSET(STDIN_FILENO, &rdfs); 
+#endif
+}
+
+inv_error_t inv_constructor_default_enable()
+{
+    inv_error_t result;
+  
+    result = inv_enable_quaternion();
+    if (result) {
+        if (result == INV_ERROR_NOT_AUTHORIZED) {
+            LOGE("Enable Quaternion failed: not authorized");
+        }
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_motion_no_motion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_gyro_tc();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_hal_outputs();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    result = inv_enable_9x_sensor_fusion();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+    return result;
+}
+
+int read_attribute_sensor(int fd, char *data, unsigned int size)
+{
+    int count = 0;
+        if (fd >=0) {
+            count = read(fd, data, size);
+            if(count < 0) {
+                MPL_LOGE("read fails with error code=%d", count);
+            }
+            close(fd);
+        }
+        return count;
+}
+
+int inv_read_temperature(long long *data)
+{
+        int count = 0;
+        int fd;
+
+        if(mFirstRead) {
+            char buf[4];
+            fd = open(filenames[ITG3500].temp_scale, O_RDONLY);
+            if(fd < 0) {
+                MPL_LOGE("errors opening tempscale");
+                return -1;
+            }
+
+            memset(buf, 0, sizeof(buf));
+
+            count = read_attribute_sensor(fd, buf, sizeof(buf));
+            if(count < 0) {
+                MPL_LOGE("errors reading temp_scale");
+                return -1;
+            }
+
+            count = sscanf(buf, "%hd", &mTempScale);
+            if(count < 1)
+                return -1;
+            MPL_LOGI("temp scale = %d", mTempScale);
+ 
+            fd = open(filenames[ITG3500].temp_offset, O_RDONLY);
+            if(fd < 0) {
+                MPL_LOGE("errors opening tempoffset");
+                return -1;
+            }
+
+            memset(buf, 0, sizeof(buf));
+
+            count = read_attribute_sensor(fd, buf, sizeof(buf));
+            if(count < 0) {
+                MPL_LOGE("errors reading temp_offset");
+                return -1;
+            }
+
+            count = sscanf(buf, "%hd", &mTempOffset);
+            if(count < 1)
+                return -1;
+            MPL_LOGI("temp offset = %d", mTempOffset);
+
+            mFirstRead = false;
+        }
+
+        char raw_buf[25];
+        short raw;
+        long long timestamp;
+        fd = open(filenames[ITG3500].temperature, O_RDONLY);
+        if(fd < 0) {
+            MPL_LOGE("errors opening temperature");
+            return -1;
+        }
+
+        memset(raw_buf, 0, sizeof(raw_buf));
+
+        count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+        if(count < 0) {
+            MPL_LOGE("errors reading temperature");
+            return -1;
+        }
+        count = sscanf(raw_buf, "%hd%lld", &raw, &timestamp);
+        if(count < -1)
+            return -1;
+        MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp);
+        MPL_LOGI("temperature offset = %d", mTempOffset);
+        MPL_LOGI("temperature scale = %d", mTempScale);
+        int adjuster = 35 + ((raw-mTempOffset)/mTempScale);
+        MPL_LOGI("pre-scaled temperature = %d", adjuster);
+        MPL_LOGI("adjusted temperature = %d", adjuster*65536);
+        //data[0] = adjuster * 65536;
+        data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f;
+        data[1] = timestamp;
+        return 0;
+}
+
+int self_test(void)
+{
+        int err = 0;
+        char str[50];
+        char x[9], y[9], z[9];
+        char pass[2];
+        int fd;
+
+        fd = open((char*)filenames[DEVICE].self_test, O_RDONLY);
+        if(fd < 0) {
+            return fd;
+        }
+        memset(str, 0, sizeof(str));
+        err = read_attribute_sensor(fd, str, sizeof(str));
+        if(err < 0) {
+            return err;
+        }
+        MPL_LOGI("self_test result: %s", str);
+        printf("Self test result: %s ", str);
+        err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass);
+        if(err < 1) {
+            return err;
+        }
+        MPL_LOGI("Bias   : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        //printf("Bias   : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+        if (atoi(pass)) {
+            MPL_LOGI("Result : PASS (1)");
+            printf("----> PASS (1)\n");
+        } else {
+            MPL_LOGI("Result : FAIL (0)");
+            printf("----> FAIL (0)\n");
+        }
+        return err;
+}
+
+/*******************************************************************************
+ ******************************* MAIN ******************************************
+ ******************************************************************************/
+
+/**
+ * @brief Main function
+ */
+int main(int argc, char *argv[])
+{
+    int key = 0;
+    int ready;
+    long accel[3];
+    short gyro[3];
+    long long timestamp = 0;
+    inv_error_t result;
+
+    char data;
+    unsigned char i;
+    int fd, bytes_read;
+    struct pollfd pfd;
+    unsigned long long time_stamp;
+    unsigned int time_H;
+    struct input_event ev[100];
+#ifdef INV_PLAYBACK_DBG
+    int logging = false;
+    FILE *logfile = NULL;
+#endif
+
+    result = inv_init_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    }
+
+    // Master Enabling.  This also turns on power_state
+    if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0)
+       printf("ERR- Failed to enable event generation\n");
+    else {
+          inv_read_data((char *)filenames[DEVICE].enable, &data);
+       printf("Event enable= %d\n", data);
+    }
+
+    // Power ON - No need after master enable above but do it anyway
+    if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0)
+       printf("ERR- Failed to set power state=1\n");
+    else {
+          inv_read_data((char *)filenames[DEVICE].power_state, &data);
+       printf("Power state: %d\n", data);
+    }
+    
+    // Turn on tap
+    if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) {
+       printf("ERR- Failed to enable Tap On\n");
+    } 
+    else {
+          inv_read_data((char *)filenames[DEVICE].tap_on, &data);
+       printf("Tap-on: %d\n", data);
+    }
+
+    // Program DMP code.  No longer required to enable tap-on first
+    if ((result = 
+            inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) {
+        printf("ERR- Failed to initiate DMP re-programming %d\n",result);
+    } else {
+         if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) {
+            printf("ERR- Failed file open to write DMP\n");
+            close(fd);
+            exit(0);
+        } else {
+            // Program 200Hz version
+            //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE));
+            //printf("Downloaded %d byte(s) to DMP\n", result);
+            result = inv_load_dmp(fd);
+            //LOG_RESULT_LOCATION(result);
+            close(fd);
+        }
+    }
+
+    // Query DMP running.  For now check by 'firmware_loaded' status 
+    if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) {
+        printf("ERR- Failed to read 'firmware_loaded'\n");
+    } else {
+        printf("Firmware Loaded/ DMP running: %d\n", data);
+    }
+
+    inv_set_fifo_processed_callback(fifoCB);
+    result = inv_constructor_default_enable();
+    result = inv_start_mpl();
+    if (result) {
+        LOG_RESULT_LOCATION(result);
+        return result;
+    } else {
+        printf ("MPL started\n");
+    }
+
+    /* Gyro Setup */
+    orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
+    inv_set_gyro_orientation_and_scale(orient,2000L<<15);
+
+    /* Accel Setup */
+    orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
+    /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data.
+     * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits.
+     * 2 * 1L<<15 * 1<<6 ==  1LL<<22
+     */
+    inv_set_accel_orientation_and_scale(orient, 1LL<<22); 
+
+    // Enable Gyro
+    if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0)
+        printf("ERR- Failed to enable Gyro\n");
+    else {
+        inv_read_data((char *)filenames[DEVICE].gyro_en, &data);
+        printf("Gyro enable: %d\n", data);
+    }
+
+    // Enable Accel
+    if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0)
+        printf("ERR- Failed to enable Accel\n");
+    else {
+        inv_read_data((char *)filenames[DEVICE].accel_en, &data);
+        printf("Accel enable: %d\n", data);
+    }
+
+    // polling for data
+    fd = open(filenames[DEVICE].buffer, O_RDONLY);
+    if(fd < 0) {
+         MPL_LOGE("Cannot open device event buffer");
+    }
+    
+    pfd.fd = fd;
+    pfd.events = POLLIN;
+
+    while (1) {
+
+       result = kbhit();
+        if (result) {
+           key = getchar();
+        } else {
+           key = 0;
+        }
+        if (key == 'l') {
+           MPL_LOGI(" 'l' - load calibration file");
+           inv_load_calibration();
+        }
+        if (key == 't') {
+           MPL_LOGI(" 't' - self test");
+           self_test();
+        }
+        if (key == 'q') {
+            MPL_LOGI(" 'q' - store calibration file");
+            inv_store_calibration();
+            break;
+        }
+#ifdef INV_PLAYBACK_DBG
+        if (key == 's') {
+            if (!logging) {
+                MPL_LOGI(" 's' - toggle logging on");
+                logfile = fopen("/data/playback.bin", "wb");
+                if (logfile) {
+                    inv_turn_on_data_logging(logfile);
+                    logging = true;
+                } else {
+                    MPL_LOGI("Error : "
+                             "cannot open log file '/data/playback.bin'");
+                }
+            } else {
+                MPL_LOGI(" 's' - toggle logging off");
+                inv_turn_off_data_logging();
+                fclose(logfile);
+                logging = false;
+            }
+            break;
+        }
+#endif
+
+        ready = poll(&pfd, 1, 100);
+        if (ready) {
+            bytes_read = read_attribute_sensor(fd, (char *)ev,
+                                      sizeof(struct input_event) * SIX_AXES);
+            //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES);
+            if (bytes_read > 0) {
+                int executed;
+
+                for (i = 0; i < bytes_read / sizeof(struct input_event); i++) {
+                    if (ev[i].type == EV_REL) {
+                        switch (ev[i].code) {
+                        case REL_X:
+                            printf("REL_X\n");
+                            gyro[0]= ev[i].value;                    //Gyro X
+                            printf("Gyro X:%5d ", gyro[0]);        
+                            break;
+                        case REL_Y:
+                            printf("REL_Y\n");
+                            gyro[1]= ev[i].value;                    //Gyro Y
+                            printf("Gyro Y:%5d ", gyro[1]);    
+                            break;
+                        case REL_Z:
+                            printf("REL_Z\n");
+                            gyro[2]= ev[i].value;                    //Gyro Z
+                            printf("Gyro Z:%5d ", gyro[2]);        
+                            break;
+                        case REL_RX:
+                            printf("REL_RX\n");
+                            accel[0]= ev[i].value;                   //Accel X
+                            printf("Accl X:%5ld ", accel[0]);
+                            break;
+                        case REL_RY:
+                            printf("REL_RY\n");
+                            accel[1]= ev[i].value;                   //Accel Y
+                            printf("Accl Y:%5ld ", accel[1]);    
+                            break;
+                        case REL_RZ:
+                            printf("REL_RZ\n");
+                            accel[2]= ev[i].value;                   //Accel Z
+                            printf("Accl Z:%5ld ", accel[2]);    
+                            break;
+                        case REL_MISC:
+                            time_H= ev[i].value;
+                            break;
+                        case REL_WHEEL:
+                            time_stamp = ((unsigned long long)(time_H) << 32) + 
+                                           (unsigned int)ev[i].value;
+                            break;
+                        default:
+                            printf("ERR- Un-recognized event code:  %5d ", ev[i].code);
+                            break;
+                        }
+                    } else {
+#if 0
+                        clock_gettime(CLOCK_MONOTONIC, &timer);
+                        curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL;                        
+                        printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL);
+#endif
+                    }
+                }
+
+                // build & process gyro + accel data
+                result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed);
+                if (result) {
+                    LOG_RESULT_LOCATION(result);
+                } else if ((result = inv_build_accel(accel, 0,
+                                                    (inv_time_t)timestamp,
+                                                    &executed))) {
+                    LOG_RESULT_LOCATION(result);
+                } 
+                if (executed) {
+                    printf("Exec on data Ok\n");
+                    s_func_cb();
+                }
+
+           } else {
+               //printf ("ERR- No data!\n");
+           }
+
+        } else { MPL_LOGV("Device not ready"); }
+    }
+    close(fd);
+    
+#ifdef INV_PLAYBACK_DBG
+    if (logging) {
+        inv_turn_off_data_logging();
+        fclose(logfile);
+    }
+#endif
+
+    return 0;
+}
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
new file mode 100644
index 0000000..5d52b21
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
new file mode 100644
index 0000000..7f6cc43
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = input_gyro$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
new file mode 100644
index 0000000..0936212
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
@@ -0,0 +1,13 @@
+#### filelist.mk for input_gyro ####
+
+# helper headers
+HEADERS := $(MPL_DIR)/authenticate.h
+#HEADERS += 
+
+# sources
+SOURCES := $(APP_DIR)/test_input_gyro.c
+
+INV_SOURCES += $(SOURCES)
+
+#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
+VPATH += $(APP_DIR)
diff --git a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
new file mode 100644
index 0000000..6fa9aab
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
@@ -0,0 +1,485 @@
+/*
+ * input interface testing
+ */
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/input.h>
+#include <linux/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "linux/ml_sysfs_helper.h"
+#include "authenticate.h"
+#include "ml_load_dmp.h"
+
+#if 0
+struct input_event {
+       struct timeval time;
+       __u16 type;
+       __u16 code;
+       __s32 value;
+};
+#endif
+
+void HandleOrient(int orient)
+{    
+    if (orient & 0x01)
+        printf("INV_X_UP\n");
+    if (orient & 0x02) 
+        printf("INV_X_DOWN\n");
+    if (orient & 0x04) 
+        printf("INV_Y_UP\n");
+    if (orient & 0x08) 
+        printf("INV_Y_DOWN\n");
+    if (orient & 0x10) 
+        printf("INV_Z_UP\n");
+    if (orient & 0x20) 
+        printf("INV_Z_DOWN\n");
+    if (orient & 0x40) 
+        printf("INV_ORIENTATION_FLIP\n");
+}
+
+void HandleTap(int tap)
+{
+    int tap_dir = tap/8;
+    int tap_num = tap%8 + 1;
+
+    switch (tap_dir) {
+        case 1:
+            printf("INV_TAP_AXIS_X_POS\n");
+            break;
+        case 2:
+            printf("INV_TAP_AXIS_X_NEG\n");
+            break;
+        case 3:
+            printf("INV_TAP_AXIS_Y_POS\n");
+            break;
+        case 4:
+            printf("INV_TAP_AXIS_Y_NEG\n");
+            break;
+        case 5:
+            printf("INV_TAP_AXIS_Z_POS\n");
+            break;
+        case 6:
+            printf("INV_TAP_AXIS_Z_NEG\n");
+            break;
+        default:
+            break;
+    }
+    printf("Tap number: %d\n", tap_num);
+}
+
+static void read_compass(int event_number) 
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned int RX;
+    unsigned long long time0, time1, time2;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", event_number);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open compass\n");
+        return;
+    }
+
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while (1) {
+    clock_gettime(CLOCK_MONOTONIC, &tsp);
+        /*read compass data here */
+        if(fd > 0){
+            ret_byte = read(fd, ev, ev_size);
+        } else {
+            ret_byte = -1;
+        }
+        time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+        if (ret_byte < 0)
+            continue;
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+                printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0));
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_X:
+                printf("CX:%5d ", ev[ii].value);
+                break;
+            case REL_Y:
+                printf("CY:%5d ", ev[ii].value);
+                break;
+            case REL_Z:
+                printf("CZ:%5d ", ev[ii].value);
+                break;
+            case REL_MISC:
+                RX = ev[ii].value;
+                break;
+            case REL_WHEEL:
+                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+                time1 = time1/1000000;
+                printf("time1: %lld ", time1);
+                break;
+            default:
+                printf("GES?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+    }
+    close(fd);
+}
+
+static void read_gesture(int num)
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned long long time;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", num);
+    MPL_LOGI("%s\n", file_name);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open gusture.\n");
+        return;
+    }
+
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while(1){
+        clock_gettime(CLOCK_MONOTONIC, &tsp);
+        if(fd > 0){
+            ret_byte = read(fd, ev, ev_size);
+        } else {
+            ret_byte = -1;
+        }
+        time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL;
+        //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3);
+        if (ret_byte < 0)
+            continue;
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL;
+                printf("mono=%lld\n", time);
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_RX:
+                printf("GESX:%5x\n", ev[ii].value);
+                HandleTap(ev[ii].value);
+                break;
+            case REL_RY:
+                printf("GESY:%5x\n", ev[ii].value);
+                HandleOrient(ev[ii].value);
+                break;
+            case REL_RZ:
+                printf("FLICK:%5x\n", ev[ii].value);
+                break;
+            default:
+                printf("?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+        }
+}
+
+static void read_gyro_accel(int num)
+{
+    int ev_size, ret_byte, ii;
+    int fd = -1;
+    unsigned int RX;
+    struct input_event ev[10];
+    char name[64];
+    char file_name[64];
+    unsigned long long time0, time1, time2;
+    struct timespec tsp;
+    ev_size = sizeof(struct input_event);
+    sprintf(file_name, "/dev/input/event%d", num);
+    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+        printf("fail to open gyro/accel\n");
+        return;
+    }    
+        
+    /* NOTE: Use this to pass device name to HAL. */
+    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    printf ("Reading From : (%s)\n", name);
+    while (1){
+        //usleep(20000);
+        ret_byte = read(fd, ev, ev_size);
+        if (ret_byte < 0)
+            continue;
+        //ret_byte = 0;
+
+        for (ii = 0; ii < ret_byte/ev_size; ii++) {
+            if(EV_REL != ev[ii].type) {
+                time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+                printf("T: %lld diff=%d ", time0, (int)(time1 - time0));
+                clock_gettime(CLOCK_MONOTONIC, &tsp);
+                time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+                printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2));
+                continue;
+            }
+            switch (ev[ii].code) {
+            case REL_X:
+                printf("GX:%5d ", ev[ii].value);
+                break;
+            case REL_Y:
+                printf("GY:%5d ", ev[ii].value);
+                break;
+            case REL_Z:
+                printf("GZ:%5d ", ev[ii].value);
+                break;
+            case REL_RX:
+                printf("AX:%5d ", ev[ii].value);
+                break;
+            case REL_RY:
+                printf("AY:%5d ", ev[ii].value);
+                break;
+            case REL_RZ:
+                printf("AZ:%5d ", ev[ii].value);
+                break;
+            case REL_MISC:
+                RX = ev[ii].value;
+                break;
+            case REL_WHEEL:
+                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+                time1 = time1/1000000;
+                printf("time1: %lld ", time1);
+                break;
+            default:
+                printf("?:  %5d ", ev[ii].code);
+                break;
+            }
+        }
+    }
+    close(fd);
+}
+int inv_sysfs_write(char *filename, long data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "w");
+    if (!fp)
+        return -errno;
+    count = fprintf(fp, "%ld", data);
+    fclose(fp);
+    return count;
+}
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "r");
+    if (!fp)
+        return -errno;
+    count = fread(data, 1, num_bytes, fp);
+    fclose(fp);
+    return count;
+}
+
+void enable_flick(char *p)
+{
+    char sysfs_file[200];
+    printf("flick:%s\n", p);
+    sprintf(sysfs_file, "%s/flick_int_on", p);
+    inv_sysfs_write(sysfs_file, 1);
+    sprintf(sysfs_file, "%s/flick_upper", p);
+    inv_sysfs_write(sysfs_file, 3147790);
+    sprintf(sysfs_file, "%s/flick_lower", p);
+    inv_sysfs_write(sysfs_file, -3147790);
+    sprintf(sysfs_file, "%s/flick_counter", p);
+    inv_sysfs_write(sysfs_file, 50);
+    sprintf(sysfs_file, "%s/flick_message_on", p);
+    inv_sysfs_write(sysfs_file, 0);
+    sprintf(sysfs_file, "%s/flick_axis", p);
+    inv_sysfs_write(sysfs_file, 2);
+}
+
+void setup_dmp(char *sysfs_path)
+{
+    char sysfs_file[200];
+    char firmware_loaded[200], dmp_path[200];
+    char dd[10];
+    
+    inv_get_dmpfile(dmp_path);
+    sprintf(sysfs_file, "%s/fifo_rate", sysfs_path);
+    inv_sysfs_write(sysfs_file, 200);    
+    sprintf(sysfs_file, "%s/FSR", sysfs_path);
+    inv_sysfs_write(sysfs_file, 2000);    
+    sprintf(sysfs_file, "%s/accl_fs", sysfs_path);
+    inv_sysfs_write(sysfs_file, 4);
+    /*
+    sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded");
+    printf("%s\n", firmware_loaded);
+    inv_sysfs_write(firmware_loaded, 0);
+    inv_sysfs_read(firmware_loaded, 1, dd);
+    printf("beforefirmware_loaded=%c\n", dd[0]);
+
+    if ((fd = open(dmp_path, O_WRONLY)) < 0 ) {
+        perror("dmp fail");
+    }
+    inv_load_dmp(fd);
+    close(fd);
+    */
+    inv_sysfs_read(firmware_loaded, 1, dd);
+    printf("firmware_loaded=%c\n", dd[0]);
+}
+void read_pedometer(char *sysfs_path){
+    int steps;
+    char sysfs_file[200];
+    char dd[4];
+    sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path);
+    inv_sysfs_read(sysfs_file, 4, dd);
+    steps = dd[0] << 8 | dd[1];
+    printf("fff=%d\n", steps);
+}
+/* The running sequence:
+    "input_gyro 2 &". 
+    This will setup the dmp firmware and let it run on background. 
+    tap and flick will work at this time. 
+    To see accelerometer data and gyro data.
+    type :
+    "input_gyro ". 
+    This will print out gyro data and accelerometer data
+    To see Compass data
+    type:
+    "input_gyro 1" */
+
+int main(int argc, char *argv[])
+{
+    unsigned int RX, i, sel;
+    unsigned char key[16];
+    struct timeval tv;
+    struct timespec tsp0, tsp1, tsp2, tsp3;
+    int event_num;
+    char sysfs_path[200];
+    char chip_name[20];
+    char sysfs_file[200];
+    if (INV_SUCCESS != inv_check_key()) {
+        printf("key check fail\n");
+        exit(0);
+    }else
+        printf("key authenticated\n");
+
+    for(i=0;i<16;i++){
+        key[i] = 0xff;
+    }
+    RX = inv_get_sysfs_key(key);
+    if(RX == INV_SUCCESS){
+        for(i=0;i<16;i++){
+            printf("%d, ", key[i]);
+        }
+        printf("\n");
+    }else{
+        printf("get key failed\n");
+    }
+    memset(sysfs_path, 0, 200);
+    memset(sysfs_file, 0, 200);
+    memset(chip_name, 0, 20);
+    inv_get_sysfs_path(sysfs_path);
+    inv_get_chip_name(chip_name);
+    printf("sysfs path: %s\n", sysfs_path);
+    printf("chip name: %s\n", chip_name);
+    /*set up driver*/
+    sprintf(sysfs_file, "%s/enable", sysfs_path);
+    inv_sysfs_write(sysfs_file, 0);
+    sprintf(sysfs_file, "%s/power_state", sysfs_path);
+    inv_sysfs_write(sysfs_file, 1);    
+    if ((getuid ()) != 0)
+        printf ("You are not root! This may not work...\n");
+    
+    if(argc ==2 )
+        sel = argv[1][0] - 0x30;
+    else
+        sel = 0;
+    switch(sel){
+    case 0:
+        printf("-------------------------------\n");
+        printf("--- log gyro and accel data ---\n");
+        printf("-------------------------------\n");
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number(chip_name, &event_num) < 0)
+            printf("mpu not installed\n");
+        else
+            read_gyro_accel(event_num);
+         break;
+         
+    case 1:
+        printf("------------------------\n");
+        printf("--- log compass data ---\n");
+        printf("------------------------\n");
+        sprintf(sysfs_file, "%s/compass_enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number("INV_COMPASS", &event_num) < 0)
+            printf("compass is not enabled\n");
+        else
+            read_compass(event_num);
+        break;
+        
+    case 2:
+        printf("--------------------\n");
+        printf("--- log gestures ---\n");
+        printf("--------------------\n");
+        setup_dmp(sysfs_path);
+        enable_flick(sysfs_path);
+        sprintf(sysfs_file, "%s/tap_on", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        sprintf(sysfs_file, "%s/enable", sysfs_path);
+        inv_sysfs_write(sysfs_file, 1);
+        if(inv_get_handler_number("INV_DMP", &event_num) < 0)
+            printf("DMP not enabled\n");
+        else
+            read_gesture(event_num);
+        break;
+        
+    case 3:
+        printf("-----------------\n");
+        printf("--- pedometer ---\n");
+        printf("-----------------\n");
+        read_pedometer(sysfs_path);
+        break;
+        
+    default:
+        printf("error choice\n");
+        break;
+    }
+    
+    gettimeofday(&tv, NULL);
+    clock_gettime(CLOCK_MONOTONIC, &tsp1);
+    clock_gettime(CLOCK_REALTIME, &tsp0);
+
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2);
+    clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3);
+    //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC,  CLOCK_REALTIME,
+    //    CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID);
+    //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec);
+    //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec);
+    //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec);
+    //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec);
+
+    //ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+    //printf ("Reading From : %s (%s)\n", argv[1], name);
+
+
+    return 0;
+}
+
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
new file mode 100644
index 0000000..33c9eef
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
new file mode 100644
index 0000000..3a055cc
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
@@ -0,0 +1,109 @@
+EXEC = inv_self_test$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+HAL_DIR    = $(INV_ROOT)/software/core/HAL
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
+PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += -nostdlib
+LFLAGS += -fpic
+LFLAGS += -Wl,--gc-sections 
+LFLAGS += -Wl,--no-whole-archive 
+LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
+LFLAGS += $(ANDROID_LINK)
+ifneq ($(PRODUCT),panda)
+LFLAGS += -rdynamic
+endif
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
new file mode 100644
index 0000000..492f47e
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for console_test ####
+
+# headers
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/inv_self_test.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
new file mode 100644
index 0000000..4f9996c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -0,0 +1,264 @@
+/**
+ *  Self Test application for Invensense's MPU6050/MPU9150.
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+
+#include "invensense.h"
+#include "ml_math_func.h"
+#include "storage_manager.h"
+#include "ml_stored_data.h"
+
+#ifndef ABS
+#define ABS(x)(((x) >= 0) ? (x) : -(x))
+#endif
+
+/** Change this key if the data being stored by this file changes */
+#define INV_DB_SAVE_KEY 53394
+
+#define FALSE   0
+#define TRUE    1
+
+#define GYRO_PASS_STATUS_BIT    0x01
+#define ACCEL_PASS_STATUS_BIT   0x02
+#define COMPASS_PASS_STATUS_BIT 0x04
+
+typedef union {
+    long l; 
+    int i;
+} bias_dtype;
+
+struct inv_sysfs_names_s {
+    char *enable;
+    char *power_state;
+    char *self_test;
+    char *temperature;
+    char *accl_bias;
+};
+
+const struct inv_sysfs_names_s mpu= {
+     /* MPU6050 & MPU9150 */
+    .enable         = "/sys/class/invensense/mpu/enable",
+    .power_state    = "/sys/class/invensense/mpu/power_state",
+    .self_test      = "/sys/class/invensense/mpu/self_test",
+    .temperature    = "/sys/class/invensense/mpu/temperature",
+    .accl_bias      = "/sys/class/invensense/mpu/accl_bias"
+};
+
+struct inv_db_save_t {
+    /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long compass_bias[3];
+    /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long gyro_bias[3];
+    /** Temperature when *gyro_bias was stored. */
+    long gyro_temp;
+    /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
+    long accel_bias[3];
+    /** Temperature when accel bias was stored. */
+    long accel_temp;
+    long gyro_temp_slope[3];
+};
+
+static struct inv_db_save_t save_data;
+
+/** This function receives the data that was stored in non-volatile memory between power off */
+static inv_error_t inv_db_load_func(const unsigned char *data)
+{
+    memcpy(&save_data, data, sizeof(save_data));
+    return INV_SUCCESS;
+}
+
+/** This function returns the data to be stored in non-volatile memory between power off */
+static inv_error_t inv_db_save_func(unsigned char *data)
+{
+    memcpy(data, &save_data, sizeof(save_data));
+    return INV_SUCCESS;
+}
+
+int inv_sysfs_write(char *filename, long data)
+{
+    FILE *fp;
+    int count;
+
+    if (!filename)
+        return -1;
+    fp = fopen(filename, "w");
+    if (!fp)
+        return -errno;
+    count = fprintf(fp, "%ld", data);
+    fclose(fp);
+    return count;
+}
+
+/**
+ *  Main Self test 
+ */
+int main(int argc, char **argv)
+{
+    FILE *fptr;
+    int self_test_status;
+    inv_error_t result;
+    bias_dtype gyro_bias[3];
+    bias_dtype accel_bias[3];
+    int axis = 0;
+    size_t packet_sz;
+    int axis_sign = 1;
+    unsigned char *buffer;
+    long timestamp;
+    int temperature=0;
+
+    // Initialize storage manager
+    inv_init_storage_manager();
+
+    // Clear out data.
+    memset(&save_data, 0, sizeof(save_data));
+    memset(gyro_bias,0, sizeof(gyro_bias));
+    memset(accel_bias,0, sizeof(accel_bias));
+
+    // Register packet to be saved.
+    result = inv_register_load_store(inv_db_load_func, inv_db_save_func,
+                                     sizeof(save_data),
+                                     INV_DB_SAVE_KEY);
+
+    // Power ON MPUxxxx chip
+    if (inv_sysfs_write(mpu.power_state, 1) <0) {
+        printf("ERR- Failed to set power state=1\n");
+    } else {
+        // Note: Driver turns on power automatically when self-test invoked
+    }
+
+    fptr = fopen(mpu.self_test, "r");
+    if (fptr != NULL) {
+        // Invoke self-test and read gyro bias
+        fscanf(fptr, "%d,%d,%d,%d",
+               &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status);
+
+        printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
+                      (self_test_status & GYRO_PASS_STATUS_BIT), 
+                      (self_test_status & ACCEL_PASS_STATUS_BIT) >>1,
+                      (self_test_status & COMPASS_PASS_STATUS_BIT) >>2);
+        printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
+        fclose(fptr);
+        
+        if (!(self_test_status & GYRO_PASS_STATUS_BIT)) {
+            // Reset gyro bias data if gyro self-test failed
+            memset(gyro_bias,0, sizeof(gyro_bias));
+            printf("Self-Test:Failed Gyro self-test\n");
+        }
+
+        if (self_test_status & ACCEL_PASS_STATUS_BIT) {
+            // Read Accel Bias
+            fptr= fopen(mpu.accl_bias, "r");
+            if (fptr != NULL) {
+                fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i);
+                printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
+                fclose(fptr);
+            } else {
+                printf("Self-Test:ERR-Couldn't read accel bias\n");
+            }
+        } else {
+            memset(accel_bias,0, sizeof(accel_bias));
+            printf("Self-Test:Failed Accel self-test\n");
+        }
+
+        // Read temperature
+        if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT))
+        {
+            fptr= fopen(mpu.temperature, "r");
+            if (fptr != NULL) {
+                fscanf(fptr,"%d %ld", &temperature, &timestamp);
+                fclose(fptr);
+            } else {
+                printf("Self-Test:ERR-Couldn't read temperature\n");
+            }
+        }
+        
+    } else {
+        printf("Self-Test:ERR-Couldn't invoke self-test\n");
+    }
+
+    // When we read gyro bias, the bias is in raw units scaled by 1000.
+    // We store the bias in raw units scaled by 2^16
+    save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f);
+    save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f);
+    save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f);
+
+    // Save temperature @ time stored.  Temperature is in degrees Celsius scaled by 2^16
+    save_data.gyro_temp = temperature * (1L << 16);
+    save_data.accel_temp = save_data.gyro_temp; 
+
+    // When we read accel bias, the bias is in raw units scaled by 1000.
+    // and it contains the gravity vector.
+    
+    // Find the orientation of the device, by looking for gravity
+    if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) {
+        axis = 1;
+    }
+    if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) {
+        axis = 2;
+    }
+    if (accel_bias[axis].l < 0) {
+        axis_sign = -1;
+    }
+    
+    // Remove gravity, gravity in raw units should be 16384 for a 2g setting.
+    // We read data scaled by 1000, so 
+    accel_bias[axis].l -= axis_sign * 4096L * 1000L;
+
+    // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16
+    save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f);
+    save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f);
+    save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f);
+
+#if 1
+    printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0],
+                    save_data.accel_bias[1], save_data.accel_bias[2]);
+    printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0],
+                    save_data.gyro_bias[1], save_data.gyro_bias[2]);
+    printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp);
+    printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp);
+#endif
+
+    // Get size of packet to store.
+    inv_get_mpl_state_size(&packet_sz);
+
+    // Create place to store data
+    buffer = (unsigned char *)malloc(packet_sz + 10);
+    if (buffer == NULL) {
+        printf("Self-Test:Can't allocate buffer\n");
+        return -1;
+    }
+
+    result = inv_save_mpl_states(buffer, packet_sz);
+    if (result) {
+        result= -1;
+    } else {
+        fptr= fopen(MLCAL_FILE, "wb+");
+        if (fptr != NULL) {
+            fwrite(buffer, 1, packet_sz, fptr);
+            fclose(fptr);
+        } else {
+            printf("Self-Test:ERR- Can't open calibration file to write - %s\n", 
+                   MLCAL_FILE);
+            result= -1;
+        }
+    }
+
+    free(buffer);
+    return result;
+}
+