Official MA 5.1.2 - RC11 Release

Signed-off-by: Rosa Chow <rchow@invensense.com>

105 files changed, 6141 insertions, 8921 deletions

diff --git a/Android.mk b/Android.mk
index 427bbb4..c42f281 100644
--- a/Android.mk
+++ b/Android.mk
@@ -1,5 +1,5 @@
 # Can't have both manta and non-manta libsensors.
-ifneq ($(filter manta, $(TARGET_DEVICE)),)
+ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
 # libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta.
 include $(call all-named-subdir-makefiles,libsensors_iio)
 else
diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk
index c3b2003..d05ec7b 100644
--- a/libsensors_iio/Android.mk
+++ b/libsensors_iio/Android.mk
@@ -15,26 +15,28 @@
 
 LOCAL_PATH := $(call my-dir)
 
-ifneq ($(TARGET_SIMULATOR),true)
-
 # InvenSense fragment of the HAL
 include $(CLEAR_VARS)
 
 LOCAL_MODULE := libinvensense_hal
 
 LOCAL_MODULE_TAGS := optional
-
+LOCAL_MODULE_OWNER := invensense
 LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
 
-ifeq ($(ENG_BUILD),1)
+VERSION_JB := true
+ifeq ($(VERSION_JB),true)
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+endif
+ifeq ($(TARGET_BUILD_VARIANT),userdebug)
 ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
 LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
 endif
 ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
 LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
 endif
-ifeq ($(COMPILE_COMPASS_YAS530),1)
-LOCAL_CFLAGS += -DCOMPASS_YAS530
+ifeq ($(COMPILE_COMPASS_YAS53x),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS53x
 endif
 ifeq ($(COMPILE_COMPASS_AK8975),1)
 LOCAL_CFLAGS += -DCOMPASS_AK8975
@@ -46,13 +48,31 @@ else # release builds, default
 LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
 endif
 
-LOCAL_SRC_FILES := SensorBase.cpp
+
+LOCAL_SRC_FILES += SensorBase.cpp
 LOCAL_SRC_FILES += MPLSensor.cpp
 LOCAL_SRC_FILES += MPLSupport.cpp
 LOCAL_SRC_FILES += InputEventReader.cpp
+
+
+ifeq ($(TARGET_BUILD_VARIANT),userdebug)
+ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
+ifeq ($(COMPILE_COMPASS_AMI306),1)
+LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
+else ifeq ($(COMPILE_COMPASS_YAS53x),1)
+LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
+else
 LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif
+else # COMPILE_INVENSENSE_COMPASS_CAL = 0
+# choose corresponding 3rd-party compass sensor file
+LOCAL_SRC_FILES += AkmSensor.cpp
+LOCAL_SRC_FILES += CompassSensor.AKM.cpp
+endif
+else # release builds, default
+LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
+endif #userdebug
 
-LOCAL_C_INCLUDES += $(LOCAL_PATH)
 LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
 LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
 LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
@@ -63,7 +83,6 @@ LOCAL_SHARED_LIBRARIES += libcutils
 LOCAL_SHARED_LIBRARIES += libutils
 LOCAL_SHARED_LIBRARIES += libdl
 LOCAL_SHARED_LIBRARIES += libmllite
-
 #Additions for SysPed
 LOCAL_SHARED_LIBRARIES += libmplmpu
 LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
@@ -72,10 +91,70 @@ LOCAL_PRELINK_MODULE := false
 
 include $(BUILD_SHARED_LIBRARY)
 
+# Build a temporary HAL that links the InvenSense .so
+include $(CLEAR_VARS)
+ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
+LOCAL_MODULE := sensors.invensense
+else
+LOCAL_MODULE := sensors.${TARGET_PRODUCT}
+endif
+LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw
+
+LOCAL_SHARED_LIBRARIES += libmplmpu
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
+
+LOCAL_PRELINK_MODULE := false
+LOCAL_MODULE_TAGS := optional
+LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+
+ifeq ($(VERSION_JB),true)
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
+endif
+
+ifeq ($(TARGET_BUILD_VARIANT),userdebug)
+ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif
+ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
+LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
+endif
+ifeq ($(COMPILE_COMPASS_YAS53x),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS53x
+endif
+ifeq ($(COMPILE_COMPASS_AK8975),1)
+LOCAL_CFLAGS += -DCOMPASS_AK8975
+endif
+ifeq ($(COMPILE_COMPASS_AMI306),1)
+LOCAL_CFLAGS += -DCOMPASS_AMI306
+endif
+else # release builds, default
+LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
+endif # userdebug
+
+ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
+#LOCAL_SRC_FILES := sensors_mpl.cpp
+else
+LOCAL_SRC_FILES := sensors_mpl.cpp
+endif
+
+
+LOCAL_SHARED_LIBRARIES := libinvensense_hal
+LOCAL_SHARED_LIBRARIES += libcutils
+LOCAL_SHARED_LIBRARIES += libutils
+LOCAL_SHARED_LIBRARIES += libdl
+LOCAL_SHARED_LIBRARIES += liblog
+LOCAL_SHARED_LIBRARIES += libmllite
+include $(BUILD_SHARED_LIBRARY)
+
 include $(CLEAR_VARS)
 LOCAL_MODULE := libmplmpu
 LOCAL_SRC_FILES := libmplmpu.so
 LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
 LOCAL_MODULE_SUFFIX := .so
 LOCAL_MODULE_CLASS := SHARED_LIBRARIES
 LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
@@ -86,10 +165,9 @@ include $(CLEAR_VARS)
 LOCAL_MODULE := libmllite
 LOCAL_SRC_FILES := libmllite.so
 LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
 LOCAL_MODULE_SUFFIX := .so
 LOCAL_MODULE_CLASS := SHARED_LIBRARIES
 LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
 OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
 include $(BUILD_PREBUILT)
-
-endif # !TARGET_SIMULATOR
diff --git a/libsensors_iio/CompassSensor.IIO.9150.cpp b/libsensors_iio/CompassSensor.IIO.9150.cpp
index d9f2e0c..b11d5cb 100644
--- a/libsensors_iio/CompassSensor.IIO.9150.cpp
+++ b/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -31,12 +31,11 @@
 #include "MPLSupport.h"
 #include "sensor_params.h"
 #include "ml_sysfs_helper.h"
-#include "local_log_def.h"
 
 #define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
 
-#if defined COMPASS_YAS530
-#   warning "Invensense compass cal with YAS530 on primary bus"
+#if defined COMPASS_YAS53x
+#   warning "Invensense compass cal with YAS53x IIO on secondary bus"
 #   define USE_MPL_COMPASS_HAL          (1)
 #   define COMPASS_NAME                 "INV_YAS530"
 #elif defined COMPASS_AK8975
@@ -44,7 +43,7 @@
 #   define USE_MPL_COMPASS_HAL          (1)
 #   define COMPASS_NAME                 "INV_AK8975"
 #elif defined INVENSENSE_COMPASS_CAL
-#   warning "Invensense compass cal with compass on secondary bus"
+#   warning "Invensense compass cal with compass IIO on secondary bus"
 #   define USE_MPL_COMPASS_HAL          (1)
 #   define COMPASS_NAME                 "INV_COMPASS"
 #else
@@ -57,11 +56,11 @@
 
 /*****************************************************************************/
 
-CompassSensor::CompassSensor() 
+CompassSensor::CompassSensor()
                   : SensorBase(NULL, NULL),
+                    compass_fd(-1),
                     mCompassTimestamp(0),
-                    mCompassInputReader(8),
-                    compass_fd(-1)
+                    mCompassInputReader(8)
 {
     VFUNC_LOG;
 
@@ -78,13 +77,13 @@ CompassSensor::CompassSensor()
 
     memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
             compassSysFs.compass_orient, getTimestamp());
     FILE *fptr;
     fptr = fopen(compassSysFs.compass_orient, "r");
     if (fptr != NULL) {
         int om[9];
-        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
                &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
                &om[6], &om[7], &om[8]);
         fclose(fptr);
@@ -107,7 +106,9 @@ CompassSensor::CompassSensor()
         LOGE("HAL:Couldn't read compass mounting matrix");
     }
 
-    enable(ID_M, 0);
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
 }
 
 CompassSensor::~CompassSensor()
@@ -130,11 +131,11 @@ int CompassSensor::getFd() const
  *  @param[in]    handle
  *                  which sensor to enable/disable.
  *  @param[in]    en
- *                  en=1, enable; 
+ *                  en=1, enable;
  *                  en=0, disable
  *  @return       if the operation is successful.
  */
-int CompassSensor::enable(int32_t handle, int en) 
+int CompassSensor::enable(int32_t handle, int en)
 {
     VFUNC_LOG;
 
@@ -142,7 +143,7 @@ int CompassSensor::enable(int32_t handle, int en)
     int tempFd;
     int res;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
             en, compassSysFs.compass_enable, getTimestamp());
     tempFd = open(compassSysFs.compass_enable, O_RDWR);
     res = errno;
@@ -156,7 +157,7 @@ int CompassSensor::enable(int32_t handle, int en)
     close(tempFd);
 
     if (en) {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, compassSysFs.compass_x_fifo_enable, getTimestamp());
         tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
         res = errno;
@@ -168,7 +169,7 @@ int CompassSensor::enable(int32_t handle, int en)
                  compassSysFs.compass_x_fifo_enable, strerror(res), res);
         }
 
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, compassSysFs.compass_y_fifo_enable, getTimestamp());
         tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
         res = errno;
@@ -180,7 +181,7 @@ int CompassSensor::enable(int32_t handle, int en)
                  compassSysFs.compass_y_fifo_enable, strerror(res), res);
         }
 
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, compassSysFs.compass_z_fifo_enable, getTimestamp());
         tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
         res = errno;
@@ -196,13 +197,13 @@ int CompassSensor::enable(int32_t handle, int en)
     return res;
 }
 
-int CompassSensor::setDelay(int32_t handle, int64_t ns) 
+int CompassSensor::setDelay(int32_t handle, int64_t ns)
 {
     VFUNC_LOG;
     int tempFd;
     int res;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
             1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
     mDelay = ns;
     if (ns == 0)
@@ -246,8 +247,8 @@ void CompassSensor::processCompassEvent(const input_event *event)
         mCachedCompassData[2] = event->value;
         break;
     }
-    
-    mCompassTimestamp = 
+
+    mCompassTimestamp =
         (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
 }
 
@@ -262,7 +263,7 @@ long CompassSensor::getSensitivity()
     VFUNC_LOG;
 
     long sensitivity;
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
             compassSysFs.compass_scale, getTimestamp());
     inv_read_data(compassSysFs.compass_scale, &sensitivity);
     return sensitivity;
@@ -310,7 +311,7 @@ int CompassSensor::readSample(long *data, int64_t *timestamp)
 
 /**
  *  @brief  This function will return the current delay for this sensor.
- *  @return delay in nanoseconds. 
+ *  @return delay in nanoseconds.
  */
 int64_t CompassSensor::getDelay(int32_t handle)
 {
@@ -333,14 +334,20 @@ void CompassSensor::fillList(struct sensor_t *list)
             return;
         }
         if(!strcmp(compass, "compass")
-                || !strcmp(compass, "INV_AK8975")
-                || !strcmp(compass, "INV_YAS530")) {
+                || !strcmp(compass, "INV_AK8975") ) {
             list->maxRange = COMPASS_AKM8975_RANGE;
             list->resolution = COMPASS_AKM8975_RESOLUTION;
             list->power = COMPASS_AKM8975_POWER;
             list->minDelay = COMPASS_AKM8975_MINDELAY;
             return;
         }
+        if(!strcmp(compass, "INV_YAS530")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            return;
+        }
         if(!strcmp(compass, "INV_AMI306")) {
             list->maxRange = COMPASS_AMI306_RANGE;
             list->resolution = COMPASS_AMI306_RESOLUTION;
@@ -383,23 +390,16 @@ int CompassSensor::inv_init_sysfs_attributes(void)
 
     // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
     // inv_get_sysfs_abs_path(sysfs_path);
-    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
-        ALOGE("CompassSensor failed get sysfs path");
-        return -1;
-    }
-
+    inv_get_sysfs_path(sysfs_path);
     inv_get_iio_trigger_path(iio_trigger_path);
 
-#if defined COMPASS_YAS530 || defined COMPASS_AK8975
+#if defined COMPASS_AK8975
     inv_get_input_number(COMPASS_NAME, &num);
     tbuf[0] = num + 0x30;
     tbuf[1] = 0;
     sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
-#if defined COMPASS_YAS530
-    strcat(sysfs_path, "/yas530");
-#else
     strcat(sysfs_path, "/ak8975");
-#endif
+
     sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
     sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
     sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
@@ -415,7 +415,7 @@ int CompassSensor::inv_init_sysfs_attributes(void)
 #endif
 
 #if 0
-    // test print sysfs paths   
+    // test print sysfs paths
     dptr = (char**)&compassSysFs;
     for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
         LOGE("HAL:sysfs path: %s", *dptr++);
diff --git a/libsensors_iio/CompassSensor.IIO.primary.cpp b/libsensors_iio/CompassSensor.IIO.primary.cpp
new file mode 100644
index 0000000..8b78338
--- /dev/null
+++ b/libsensors_iio/CompassSensor.IIO.primary.cpp
@@ -0,0 +1,630 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_NDEBUG 0
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+#include <string.h>
+
+#include "CompassSensor.IIO.primary.h"
+#include "sensors.h"
+#include "MPLSupport.h"
+#include "sensor_params.h"
+#include "ml_sysfs_helper.h"
+//#include "log.h"
+
+#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
+
+#if defined COMPASS_AK8975
+#   warning "Invensense compass cal with AK8975 on primary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_AK8975"
+#elif defined INVENSENSE_COMPASS_CAL
+
+#if defined COMPASS_YAS53x
+#   warning "Invensense compass cal with YAS53x IIO on primary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "yas53" // prefix for YAS53[023]
+#elif defined COMPASS_AMI306
+#   warning "Invensense compass cal with AMI306 IIO on primary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "ami306"
+#else
+#   warning "Invensense compass cal with compass on secondary bus"
+#   define USE_MPL_COMPASS_HAL          (1)
+#   define COMPASS_NAME                 "INV_COMPASS"
+#endif
+
+#else
+#   warning "third party compass cal HAL"
+#   define USE_MPL_COMPASS_HAL          (0)
+// TODO: change to vendor's name
+#   define COMPASS_NAME                 "AKM8975"
+#endif
+
+/******************************************************************************/
+
+CompassSensor::CompassSensor()
+                  : SensorBase(COMPASS_NAME, NULL),
+                    mCompassTimestamp(0),
+                    mCompassInputReader(8)
+#ifdef COMPASS_YAS53x
+                    , mCoilsResetFd(0)
+#endif
+{
+    FILE *fptr;
+
+    VFUNC_LOG;
+
+    /*
+    char temp_sysfs_path[30];
+    inv_get_sysfs_path(temp_sysfs_path);
+    LOGI("sysfs: %s", temp_sysfs_path);
+    */
+
+#ifdef COMPASS_YAS53x
+    /* for YAS53x compasses, dev_name is just a prefix,
+       we need to find the actual name */
+    if (fill_dev_full_name_by_prefix(dev_name,
+            dev_full_name, sizeof(dev_full_name) / sizeof(dev_full_name[0]))) {
+        LOGE("Cannot find Yamaha device with prefix name '%s' - "
+             "magnetometer will likely not work.", dev_name);
+    }
+#else
+    strncpy(dev_full_name, dev_name,
+            sizeof(dev_full_name) / sizeof(dev_full_name[0]));
+#endif
+
+    if (inv_init_sysfs_attributes()) {
+        LOGE("Error Instantiating Compass\n");
+        return;
+    }
+
+    if (!strcmp(dev_full_name, "INV_COMPASS")) {
+        mI2CBus = COMPASS_BUS_SECONDARY;
+    } else {
+        mI2CBus = COMPASS_BUS_PRIMARY;
+    }
+
+    memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
+
+    if (!isIntegrated()) {
+        enable(ID_M, 0);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:compass name: %s", dev_full_name);
+    enable_iio_sysfs();
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            compassSysFs.compass_orient, getTimestamp());
+    fptr = fopen(compassSysFs.compass_orient, "r");
+    if (fptr != NULL) {
+        int om[9];
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+               &om[6], &om[7], &om[8]);
+        fclose(fptr);
+
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:compass mounting matrix: "
+                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
+                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
+
+        mCompassOrientation[0] = om[0];
+        mCompassOrientation[1] = om[1];
+        mCompassOrientation[2] = om[2];
+        mCompassOrientation[3] = om[3];
+        mCompassOrientation[4] = om[4];
+        mCompassOrientation[5] = om[5];
+        mCompassOrientation[6] = om[6];
+        mCompassOrientation[7] = om[7];
+        mCompassOrientation[8] = om[8];
+    } else {
+        LOGE("HAL:Couldn't read compass mounting matrix");
+    }
+
+#ifdef COMPASS_YAS53x
+    mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+");
+    if (fptr == NULL) {
+        LOGE("HAL:Couldn't read compass overunderflow");
+    }
+#endif
+}
+
+void CompassSensor::enable_iio_sysfs()
+{
+    VFUNC_LOG;
+
+    int tempFd = 0;
+    char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
+    FILE *tempFp = NULL;
+    const char* compass = dev_full_name;
+
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:echo 1 > %s (%lld)",
+            compassSysFs.in_timestamp_en, getTimestamp());
+    tempFd = open(compassSysFs.in_timestamp_en, O_RDWR);
+    if(tempFd < 0) {
+        LOGE("HAL:could not open %s timestamp enable", compass);
+    } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
+        LOGE("HAL:could not enable timestamp");
+    }
+
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)",
+            compassSysFs.trigger_name, getTimestamp());
+    tempFp = fopen(compassSysFs.trigger_name, "r");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open %s trigger name", compass);
+    } else {
+        if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
+            LOGE("HAL:could not read trigger name");
+        }
+        fclose(tempFp);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:echo %s > %s (%lld)",
+            iio_trigger_name, compassSysFs.current_trigger, getTimestamp());
+    tempFp = fopen(compassSysFs.current_trigger, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open current trigger");
+    } else {
+        if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
+            LOGE("HAL:could not write current trigger");
+        }
+        fclose(tempFp);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:echo %d > %s (%lld)",
+            IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp());
+    tempFp = fopen(compassSysFs.buffer_length, "w");
+    if (tempFp == NULL) {
+        LOGE("HAL:could not open buffer length");
+    } else {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
+            LOGE("HAL:could not write buffer length");
+        }
+        fclose(tempFp);
+    }
+
+    sprintf(iio_device_node, "%s%d", "/dev/iio:device",
+            find_type_by_name(compass, "iio:device"));
+    compass_fd = open(iio_device_node, O_RDONLY);
+    int res = errno;
+    if (compass_fd < 0) {
+        LOGE("HAL:could not open '%s' iio device node in path '%s' - "
+             "error '%s' (%d)",
+             compass, iio_device_node, strerror(res), res);
+    } else {
+        LOGV_IF(EXTRA_VERBOSE,
+                "HAL:iio %s, compass_fd opened : %d", compass, compass_fd);
+    }
+
+    /* TODO: need further tests for optimization to reduce context-switch
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
+            compassSysFs.compass_x_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_x_fifo_enable, strerror(res), res);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
+            compassSysFs.compass_y_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_y_fifo_enable, strerror(res), res);
+    }
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
+            compassSysFs.compass_z_fifo_enable, getTimestamp());
+    tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, 1);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.compass_z_fifo_enable, strerror(res), res);
+    }
+    */
+}
+
+CompassSensor::~CompassSensor()
+{
+    VFUNC_LOG;
+
+    free(pathP);
+    if( compass_fd > 0)
+        close(compass_fd);
+#ifdef COMPASS_YAS53x
+    if( mCoilsResetFd != NULL )
+        fclose(mCoilsResetFd);
+#endif
+}
+
+int CompassSensor::getFd() const
+{
+    VHANDLER_LOG;
+    return compass_fd;
+}
+
+/**
+ *  @brief        This function will enable/disable sensor.
+ *  @param[in]    handle
+ *                  which sensor to enable/disable.
+ *  @param[in]    en
+ *                  en=1, enable;
+ *                  en=0, disable
+ *  @return       if the operation is successful.
+ */
+int CompassSensor::enable(int32_t handle, int en)
+{
+    VFUNC_LOG;
+
+    mEnable = en;
+    int tempFd;
+    int res;
+
+    /* reset master enable */
+    res = masterEnable(0);
+    if (res < 0) {
+        return res;
+    }
+
+    if (en) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, compassSysFs.compass_x_fifo_enable, getTimestamp());
+        tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_x_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, compassSysFs.compass_y_fifo_enable, getTimestamp());
+        tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_y_fifo_enable, strerror(res), res);
+        }
+
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, compassSysFs.compass_z_fifo_enable, getTimestamp());
+        tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, en);
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                 compassSysFs.compass_z_fifo_enable, strerror(res), res);
+        }
+
+        res = masterEnable(en);
+        if (res < en) {
+            return res;
+        }
+    }
+
+    return res;
+}
+
+int CompassSensor::masterEnable(int en) {
+    VFUNC_LOG;
+
+    int res = 0;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, compassSysFs.chip_enable, getTimestamp());
+    int tempFd = open(compassSysFs.chip_enable, O_RDWR);
+    res = errno;
+    if(tempFd < 0){
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             compassSysFs.chip_enable, strerror(res), res);
+        return res;
+    }
+    res = enable_sysfs_sensor(tempFd, en);
+    return res;
+}
+
+int CompassSensor::setDelay(int32_t handle, int64_t ns)
+{
+    VFUNC_LOG;
+    int tempFd;
+    int res;
+
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+            1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
+    mDelay = ns;
+    if (ns == 0)
+        return -1;
+    tempFd = open(compassSysFs.compass_rate, O_RDWR);
+    res = write_attribute_sensor(tempFd, 1000000000.f / ns);
+    if(res < 0) {
+        LOGE("HAL:Compass update delay error");
+    }
+    return res;
+}
+
+/**
+    @brief      This function will return the state of the sensor.
+    @return     1=enabled; 0=disabled
+**/
+int CompassSensor::getEnable(int32_t handle)
+{
+    VFUNC_LOG;
+    return mEnable;
+}
+
+/* use for Invensense compass calibration */
+#define COMPASS_EVENT_DEBUG (0)
+void CompassSensor::processCompassEvent(const input_event *event)
+{
+    VHANDLER_LOG;
+
+    switch (event->code) {
+    case EVENT_TYPE_ICOMPASS_X:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
+        mCachedCompassData[0] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Y:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
+        mCachedCompassData[1] = event->value;
+        break;
+    case EVENT_TYPE_ICOMPASS_Z:
+        LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
+        mCachedCompassData[2] = event->value;
+        break;
+    }
+
+    mCompassTimestamp =
+        (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
+}
+
+void CompassSensor::getOrientationMatrix(signed char *orient)
+{
+    VFUNC_LOG;
+    memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
+}
+
+long CompassSensor::getSensitivity()
+{
+    VFUNC_LOG;
+
+    long sensitivity;
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            compassSysFs.compass_scale, getTimestamp());
+    inv_read_data(compassSysFs.compass_scale, &sensitivity);
+    return sensitivity;
+}
+
+/**
+    @brief         This function is called by sensors_mpl.cpp
+                   to read sensor data from the driver.
+    @param[out]    data      sensor data is stored in this variable. Scaled such that
+                             1 uT = 2^16
+    @para[in]      timestamp data's timestamp
+    @return        1, if 1   sample read, 0, if not, negative if error
+ */
+int CompassSensor::readSample(long *data, int64_t *timestamp) {
+    VFUNC_LOG;
+
+    int i;
+    char *rdata = mIIOBuffer;
+
+    size_t rsize = read(compass_fd, rdata, (8 * mEnable + 8) * 1);
+
+    if (!mEnable) {
+        rsize = read(compass_fd, rdata, (8 + 8) * IIO_BUFFER_LENGTH);
+        // LOGI("clear buffer with size: %d", rsize);
+    }
+/*
+    LOGI("get one sample of AMI IIO data with size: %d", rsize);
+    LOGI_IF(mEnable, "compass x/y/z: %d/%d/%d", *((short *) (rdata + 0)),
+        *((short *) (rdata + 2)), *((short *) (rdata + 4)));
+*/
+    if (mEnable) {
+        for (i = 0; i < 3; i++) {
+            data[i] = *((short *) (rdata + i * 2));
+        }
+        *timestamp = *((long long *) (rdata + 8 * mEnable));
+    }
+
+    return mEnable;
+}
+
+/**
+ *  @brief  This function will return the current delay for this sensor.
+ *  @return delay in nanoseconds.
+ */
+int64_t CompassSensor::getDelay(int32_t handle)
+{
+    VFUNC_LOG;
+    return mDelay;
+}
+
+void CompassSensor::fillList(struct sensor_t *list)
+{
+    VFUNC_LOG;
+
+    const char *compass = dev_full_name;
+
+    if (compass) {
+        if(!strcmp(compass, "INV_COMPASS")) {
+            list->maxRange = COMPASS_MPU9150_RANGE;
+            list->resolution = COMPASS_MPU9150_RESOLUTION;
+            list->power = COMPASS_MPU9150_POWER;
+            list->minDelay = COMPASS_MPU9150_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "compass")
+                || !strcmp(compass, "INV_AK8975")) {
+            list->maxRange = COMPASS_AKM8975_RANGE;
+            list->resolution = COMPASS_AKM8975_RESOLUTION;
+            list->power = COMPASS_AKM8975_POWER;
+            list->minDelay = COMPASS_AKM8975_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "ami306")) {
+            list->maxRange = COMPASS_AMI306_RANGE;
+            list->resolution = COMPASS_AMI306_RESOLUTION;
+            list->power = COMPASS_AMI306_POWER;
+            list->minDelay = COMPASS_AMI306_MINDELAY;
+            return;
+        }
+        if(!strcmp(compass, "yas530")
+                || !strcmp(compass, "yas532")
+                || !strcmp(compass, "yas533")) {
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
+            return;
+        }
+    }
+
+    LOGE("HAL:unknown compass id %s -- "
+         "params default to ak8975 and might be wrong.",
+         compass);
+    list->maxRange = COMPASS_AKM8975_RANGE;
+    list->resolution = COMPASS_AKM8975_RESOLUTION;
+    list->power = COMPASS_AKM8975_POWER;
+    list->minDelay = COMPASS_AKM8975_MINDELAY;
+}
+
+#ifdef COMPASS_YAS53x
+/* Read sysfs entry to determine whether overflow had happend
+   then write to sysfs to reset to zero */
+int CompassSensor::checkCoilsReset()
+{
+    int result=-1;
+    VFUNC_LOG;
+
+    if(mCoilsResetFd != NULL) {
+        int attr;
+        rewind(mCoilsResetFd);
+        fscanf(mCoilsResetFd, "%d", &attr);
+        if(attr == 0)
+            return 0;
+        else {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:overflow detected");
+            rewind(mCoilsResetFd);
+            if( fprintf(mCoilsResetFd, "%d", 0) < 0) {
+                LOGE("HAL:could not write overunderflow");
+            }
+            else
+                return 1;
+        }
+    }
+    else {
+        LOGE("HAL:could not read overunderflow");
+    }
+    return result;
+}
+#endif
+
+int CompassSensor::inv_init_sysfs_attributes(void)
+{
+    VFUNC_LOG;
+
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN],
+         iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2];
+    char *sptr;
+    char **dptr;
+    int num;
+    const char* compass = dev_full_name;
+
+    pathP = (char*)malloc(
+                    sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = pathP;
+    dptr = (char**)&compassSysFs;
+    if (sptr == NULL)
+        return -1;
+
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < COMPASS_MAX_SYSFS_ATTRB);
+
+    // get proper (in absolute/relative) IIO path & build sysfs paths
+    sprintf(sysfs_path, "%s%d", "/sys/bus/iio/devices/iio:device",
+        find_type_by_name(compass, "iio:device"));
+    sprintf(iio_trigger_path, "%s%d", "/sys/bus/iio/devices/trigger",
+        find_type_by_name(compass, "iio:device"));
+
+#if defined COMPASS_AK8975
+    inv_get_input_number(compass, &num);
+    tbuf[0] = num + 0x30;
+    tbuf[1] = 0;
+    sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+    strcat(sysfs_path, "/ak8975");
+
+    sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+#else /* IIO */
+    sprintf(compassSysFs.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(compassSysFs.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
+    sprintf(compassSysFs.trigger_name, "%s%s", iio_trigger_path, "/name");
+    sprintf(compassSysFs.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
+    sprintf(compassSysFs.buffer_length, "%s%s", sysfs_path, "/buffer/length");
+
+    sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en");
+    sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en");
+    sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en");
+    sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency");
+    sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
+    sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
+
+#if defined COMPASS_YAS53x
+    sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow");
+#endif
+#endif
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&compassSysFs;
+    LOGI("sysfs path base: %s", sysfs_path);
+    LOGI("trigger sysfs path base: %s", iio_trigger_path);
+    for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
+        LOGE("HAL:sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
diff --git a/libsensors_iio/CompassSensor.IIO.primary.h b/libsensors_iio/CompassSensor.IIO.primary.h
new file mode 100644
index 0000000..844615c
--- /dev/null
+++ b/libsensors_iio/CompassSensor.IIO.primary.h
@@ -0,0 +1,117 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef COMPASS_SENSOR_H
+#define COMPASS_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+// TODO fixme, need input_event
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+#define MAX_CHIP_ID_LEN (20)
+
+class CompassSensor : public SensorBase {
+
+public:
+    CompassSensor();
+    virtual ~CompassSensor();
+
+    virtual int getFd() const;
+    virtual int enable(int32_t handle, int enabled);
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int getEnable(int32_t handle);
+    virtual int64_t getDelay(int32_t handle);
+
+    // unnecessary for MPL
+    virtual int readEvents(sensors_event_t *data, int count) { return 0; }
+
+    int readSample(long *data, int64_t *timestamp);
+    int providesCalibration() { return 0; }
+    void getOrientationMatrix(signed char *orient);
+    long getSensitivity();
+    int getAccuracy() { return 0; }
+    void fillList(struct sensor_t *list);
+    int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); }
+#ifdef COMPASS_YAS53x
+    int checkCoilsReset(void);
+#endif
+
+private:
+    enum CompassBus {
+        COMPASS_BUS_PRIMARY = 0,
+        COMPASS_BUS_SECONDARY = 1
+    } mI2CBus;
+
+    struct sysfs_attrbs {
+       char *chip_enable;
+       char *in_timestamp_en;
+       char *trigger_name;
+       char *current_trigger;
+       char *buffer_length;
+
+       char *compass_enable;
+       char *compass_x_fifo_enable;
+       char *compass_y_fifo_enable;
+       char *compass_z_fifo_enable;
+       char *compass_rate;
+       char *compass_scale;
+       char *compass_orient;
+#ifdef COMPASS_YAS53x
+       char *compass_attr_1;
+#endif
+    } compassSysFs;
+
+    char dev_full_name[20];
+
+    // implementation specific
+    signed char mCompassOrientation[9];
+    long mCachedCompassData[3];
+    int64_t mCompassTimestamp;
+    InputEventCircularReader mCompassInputReader;
+    int compass_fd;
+    int64_t mDelay;
+    int mEnable;
+    char *pathP;
+
+    char mIIOBuffer[(8 + 8) * IIO_BUFFER_LENGTH];
+
+    int masterEnable(int en);
+    void enable_iio_sysfs(void);
+    void processCompassEvent(const input_event *event);
+    int inv_init_sysfs_attributes(void);
+
+#ifdef COMPASS_YAS53x
+    FILE *mCoilsResetFd;
+#endif
+};
+
+/*****************************************************************************/
+
+#endif  // COMPASS_SENSOR_H
diff --git a/libsensors_iio/InputEventReader.cpp b/libsensors_iio/InputEventReader.cpp
index ca0a61a..d3e0fe6 100644
--- a/libsensors_iio/InputEventReader.cpp
+++ b/libsensors_iio/InputEventReader.cpp
@@ -1,105 +1,104 @@
-/*

-* Copyright (C) 2012 Invensense, Inc.

-*

-* Licensed under the Apache License, Version 2.0 (the "License");

-* you may not use this file except in compliance with the License.

-* You may obtain a copy of the License at

-*

-*      http://www.apache.org/licenses/LICENSE-2.0

-*

-* Unless required by applicable law or agreed to in writing, software

-* distributed under the License is distributed on an "AS IS" BASIS,

-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-* See the License for the specific language governing permissions and

-* limitations under the License.

-*/

-

-#define LOG_NDEBUG 0

-

-#include <stdint.h>

-#include <errno.h>

-#include <unistd.h>

-#include <poll.h>

-

-#include <sys/cdefs.h>

-#include <sys/types.h>

-

-#include <linux/input.h>

-

-#include <cutils/log.h>

-

-#include "InputEventReader.h"

-#include "local_log_def.h"

-

-/*****************************************************************************/

-

-struct input_event;

-

-InputEventCircularReader::InputEventCircularReader(size_t numEvents)

-    : mBuffer(new input_event[numEvents * 2]),

-      mBufferEnd(mBuffer + numEvents),

-      mHead(mBuffer),

-      mCurr(mBuffer),

-      mFreeSpace(numEvents)

-{

-}

-

-InputEventCircularReader::~InputEventCircularReader()

-{

-    delete [] mBuffer;

-}

-

-#define INPUT_EVENT_DEBUG (0)

-ssize_t InputEventCircularReader::fill(int fd)

-{

-    size_t numEventsRead = 0;

-    LOGV_IF(INPUT_EVENT_DEBUG, 

-            "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);

-    if (mFreeSpace) {

-        const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));

-        if (nread < 0 || nread % sizeof(input_event)) {

-            //LOGE("Partial event received nread=%d, required=%d", 

-            //     nread, sizeof(input_event));

-            //LOGE("FD trying to read is: %d");

-            // we got a partial event!!

-            if (INPUT_EVENT_DEBUG) {

-                LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n", 

-                        __PRETTY_FUNCTION__);

-                LOGV_IF(nread % sizeof(input_event), 

-                        "DEBUG:%s exit nread %% sizeof(input_event)\n", 

-                        __PRETTY_FUNCTION__);

-            }

-            return (nread < 0 ? -errno : -EINVAL);

-        }

-

-        numEventsRead = nread / sizeof(input_event);

-        if (numEventsRead) {

-            mHead += numEventsRead;

-            mFreeSpace -= numEventsRead;

-            if (mHead > mBufferEnd) {

-                size_t s = mHead - mBufferEnd;

-                memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));

-                mHead = mBuffer + s;

-            }

-        }

-    }

-

-    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);

-    return numEventsRead;

-}

-

-ssize_t InputEventCircularReader::readEvent(input_event const** events)

-{

-    *events = mCurr;

-    ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;

-    return available ? 1 : 0;

-}

-

-void InputEventCircularReader::next()

-{

-    mCurr++;

-    mFreeSpace++;

-    if (mCurr >= mBufferEnd) {

-        mCurr = mBuffer;

-    }

-}

+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+
+#include <stdint.h>
+#include <errno.h>
+#include <unistd.h>
+#include <poll.h>
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <cutils/log.h>
+
+#include "InputEventReader.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+InputEventCircularReader::InputEventCircularReader(size_t numEvents)
+    : mBuffer(new input_event[numEvents * 2]),
+      mBufferEnd(mBuffer + numEvents),
+      mHead(mBuffer),
+      mCurr(mBuffer),
+      mFreeSpace(numEvents)
+{
+}
+
+InputEventCircularReader::~InputEventCircularReader()
+{
+    delete [] mBuffer;
+}
+
+#define INPUT_EVENT_DEBUG (0)
+ssize_t InputEventCircularReader::fill(int fd)
+{
+    size_t numEventsRead = 0;
+    LOGV_IF(INPUT_EVENT_DEBUG,
+            "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
+    if (mFreeSpace) {
+        const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
+        if (nread < 0 || nread % sizeof(input_event)) {
+            //LOGE("Partial event received nread=%d, required=%d",
+            //     nread, sizeof(input_event));
+            //LOGE("FD trying to read is: %d");
+            // we got a partial event!!
+            if (INPUT_EVENT_DEBUG) {
+                LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n",
+                        __PRETTY_FUNCTION__);
+                LOGV_IF(nread % sizeof(input_event),
+                        "DEBUG:%s exit nread %% sizeof(input_event)\n",
+                        __PRETTY_FUNCTION__);
+            }
+            return (nread < 0 ? -errno : -EINVAL);
+        }
+
+        numEventsRead = nread / sizeof(input_event);
+        if (numEventsRead) {
+            mHead += numEventsRead;
+            mFreeSpace -= numEventsRead;
+            if (mHead > mBufferEnd) {
+                size_t s = mHead - mBufferEnd;
+                memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
+                mHead = mBuffer + s;
+            }
+        }
+    }
+
+    LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);
+    return numEventsRead;
+}
+
+ssize_t InputEventCircularReader::readEvent(input_event const** events)
+{
+    *events = mCurr;
+    ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
+    return available ? 1 : 0;
+}
+
+void InputEventCircularReader::next()
+{
+    mCurr++;
+    mFreeSpace++;
+    if (mCurr >= mBufferEnd) {
+        mCurr = mBuffer;
+    }
+}
diff --git a/libsensors_iio/InputEventReader.h b/libsensors_iio/InputEventReader.h
index 11c4e73..7cd2ee2 100644
--- a/libsensors_iio/InputEventReader.h
+++ b/libsensors_iio/InputEventReader.h
@@ -22,6 +22,8 @@
 #include <sys/cdefs.h>
 #include <sys/types.h>
 
+#include "SensorBase.h"

+

 /*****************************************************************************/
 
 struct input_event;
diff --git a/libsensors_iio/MPLSensor.cpp b/libsensors_iio/MPLSensor.cpp
index e23ecc9..7c750c1 100644
--- a/libsensors_iio/MPLSensor.cpp
+++ b/libsensors_iio/MPLSensor.cpp
@@ -34,6 +34,7 @@
 #include <utils/String8.h>
 #include <string.h>
 #include <linux/input.h>
+#include <utils/Atomic.h>
 
 #include "MPLSensor.h"
 #include "MPLSupport.h"
@@ -46,7 +47,6 @@
 #include "ml_sysfs_helper.h"
 
 // #define TESTING
-// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */
 
 #ifdef THIRD_PARTY_ACCEL
 #   warning "Third party accel"
@@ -80,11 +80,18 @@ static int hertz_request = 200;
 #define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
 #define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
 
+#define RATE_200HZ                      5000000LL
+#define RATE_15HZ                       66667000LL
+#define RATE_5HZ                        200000000LL
+
 static struct sensor_t sSensorList[] =
 {
     {"MPL Gyroscope", "Invensense", 1,
      SENSORS_GYROSCOPE_HANDLE,
      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
+    {"MPL Raw Gyroscope", "Invensense", 1,
+     SENSORS_RAW_GYROSCOPE_HANDLE,
+     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
     {"MPL Accelerometer", "Invensense", 1,
      SENSORS_ACCELERATION_HANDLE,
      SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}},
@@ -103,6 +110,12 @@ static struct sensor_t sSensorList[] =
     {"MPL Gravity", "Invensense", 1,
      SENSORS_GRAVITY_HANDLE,
      SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    {"MPL Screen Orientation", "Invensense ", 1,
+     SENSORS_SCREEN_ORIENTATION_HANDLE,
+     SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, {}},
+#endif
 };
 
 MPLSensor *MPLSensor::gMPLSensor = NULL;
@@ -134,17 +147,20 @@ static FILE *logfile = NULL;
  * MPLSensor class implementation
  ******************************************************************************/
 
-MPLSensor::MPLSensor(CompassSensor *compass)
+// following extended initializer list would only be available with -std=c++11 or -std=gnu+11
+MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
                        : SensorBase(NULL, NULL),
-                         mMplSensorInitialized(false),
                          mNewData(0),
                          mMasterSensorMask(INV_ALL_SENSORS),
-                         mLocalSensorMask(ALL_MPL_SENSORS_NP),
+                         mLocalSensorMask(0),
                          mPollTime(-1),
                          mHaveGoodMpuCal(0),
                          mGyroAccuracy(0),
                          mAccelAccuracy(0),
+                         mCompassAccuracy(0),
                          mSampleCount(0),
+                         dmp_orient_fd(-1),
+                         mDmpOrientationEnabled(0),
                          mEnabled(0),
                          mOldEnabledMask(0),
                          mAccelInputReader(4),
@@ -155,8 +171,6 @@ MPLSensor::MPLSensor(CompassSensor *compass)
                          mAccelScale(2),
                          mPendingMask(0),
                          mSensorMask(0),
-                         mGyroOrientation{0},
-                         mAccelOrientation{0},
                          mFeatureActiveMask(0) {
     VFUNC_LOG;
 
@@ -171,7 +185,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     mCompassSensor = compass;
 
     LOGV_IF(EXTRA_VERBOSE,
-            "HAL:MPLSensor constructor : numSensors = %d", numSensors);
+            "HAL:MPLSensor constructor : NUMSENSORS = %d", NUMSENSORS);
 
     pthread_mutex_init(&mMplMutex, NULL);
     pthread_mutex_init(&mHALMutex, NULL);
@@ -184,10 +198,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
 #endif
 
     /* setup sysfs paths */
-    if(inv_init_sysfs_attributes()) {
-        ALOGE("MPLSensor failed to init sysfs attributes");
-        return;
-    }
+    inv_init_sysfs_attributes();
 
     /* get chip name */
     if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
@@ -204,11 +215,10 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     /* reset driver master enable */
     masterEnable(0);
 
-    /* Load DMP image if capable, ie. MPU6xxx/9xxx */
-    // TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
-    loadDMP();
-#endif
+    if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+        /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+        loadDMP();
+    }
 
     /* open temperature fd for temp comp */
     LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
@@ -269,6 +279,11 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
     mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 
+    mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
+    mPendingEvents[RawGyro].sensor = ID_RG;
+    mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
+    mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
     mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
     mPendingEvents[Accelerometer].sensor = ID_A;
     mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
@@ -292,11 +307,12 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     mHandlers[LinearAccel] = &MPLSensor::laHandler;
     mHandlers[Gravity] = &MPLSensor::gravHandler;
     mHandlers[Gyro] = &MPLSensor::gyroHandler;
+    mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
     mHandlers[Accelerometer] = &MPLSensor::accelHandler;
     mHandlers[MagneticField] = &MPLSensor::compassHandler;
     mHandlers[Orientation] = &MPLSensor::orienHandler;
 
-    for (int i = 0; i < numSensors; i++) {
+    for (int i = 0; i < NUMSENSORS; i++) {
         mDelays[i] = 0;
     }
 
@@ -306,13 +322,35 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     /* setup MPL */
     inv_constructor_init();
 
-    /* load calibration file from /data/cal.bin */
+    /* load calibration file from /data/inv_cal_data.bin */
     rv = inv_load_calibration();
     if(rv == INV_SUCCESS)
         LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
     else
         LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
 
+    /* Takes external Accel Calibration Load Method */
+    if( m_pt2AccelCalLoadFunc != NULL)
+    {
+        long accel_offset[3];
+        long tmp_offset[3];
+        int result = m_pt2AccelCalLoadFunc(accel_offset);
+        if(result)
+            LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
+        else
+        {
+            LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+            inv_set_accel_bias(accel_offset, mAccelAccuracy);
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+        }
+    }
+    /* End of Accel Calibration Load Method */
+
     inv_set_device_properties();
 
     /* disable driver master enable the first sensor goes on */
@@ -320,67 +358,84 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     enableGyro(0);
     enableAccel(0);
     enableCompass(0);
+
+    if ( isLowPowerQuatEnabled() ) {
+        enableLPQuaternion(0);
+    }
+
     onPower(0);
 
+    if (isDmpDisplayOrientationOn()) {
+        enableDmpOrientation(isDmpDisplayOrientationOn());
+        enableDmpOrientation(!isDmpScreenAutoRotationEnabled()
+            && isDmpDisplayOrientationOn());
+    }
+
 #ifdef INV_PLAYBACK_DBG
     logfile = fopen("/data/playback.bin", "wb");
     if (logfile)
         inv_turn_on_data_logging(logfile);
 #endif
-
-    mMplSensorInitialized = true;
 }
 
-
-void MPLSensor::enable_iio_sysfs() {
+void MPLSensor::enable_iio_sysfs()
+{
     VFUNC_LOG;
 
-    int tempFd = 0;
     char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
     FILE *tempFp = NULL;
 
-    LOGV_IF(SYSFS_VERBOSE, 
-            "HAL:sysfs:echo 1 > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
             mpu.in_timestamp_en, getTimestamp());
-    tempFd = open(mpu.in_timestamp_en, O_RDWR);
-    if(tempFd < 0) {
+    // fopen()/open() would both be okay for sysfs access
+    // developers could choose what they want
+    // with fopen(), benefits from fprintf()/fscanf() would be available
+    tempFp = fopen(mpu.in_timestamp_en, "w");
+    if (tempFp == NULL) {
         LOGE("HAL:could not open timestamp enable");
-    } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
-        LOGE("HAL:could not enable timestamp");
+    } else {
+        if(fprintf(tempFp, "%d", 1) < 0) {
+            LOGE("HAL:could not enable timestamp");
+        }
+        fclose(tempFp);
     }
 
-    LOGV_IF(SYSFS_VERBOSE, 
-            "HAL:sysfs:cat %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE,
+            "HAL:sysfs:cat %s (%lld)",
             mpu.trigger_name, getTimestamp());
     tempFp = fopen(mpu.trigger_name, "r");
     if (tempFp == NULL) {
         LOGE("HAL:could not open trigger name");
-    } else if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
-        LOGE("HAL:could not read trigger name");
+    } else {
+        if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
+            LOGE("HAL:could not read trigger name");
+        }
+        fclose(tempFp);
     }
-    fclose(tempFp);
 
-    LOGV_IF(SYSFS_VERBOSE, 
-            "HAL:sysfs:echo %s > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
             iio_trigger_name, mpu.current_trigger, getTimestamp());
     tempFp = fopen(mpu.current_trigger, "w");
     if (tempFp == NULL) {
         LOGE("HAL:could not open current trigger");
-    } else if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
-        LOGE("HAL:could not write current trigger");
+    } else {
+        if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
+            LOGE("HAL:could not write current trigger");
+        }
+        fclose(tempFp);
     }
-    fclose(tempFp);
 
-    LOGV_IF(SYSFS_VERBOSE, 
-            "HAL:sysfs:echo %d > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
             IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
     tempFp = fopen(mpu.buffer_length, "w");
     if (tempFp == NULL) {
         LOGE("HAL:could not open buffer length");
-    } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
-        LOGE("HAL:could not write buffer length");
+    } else {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
+            LOGE("HAL:could not write buffer length");
+        }
+        fclose(tempFp);
     }
-    fclose(tempFp);
 
     inv_get_iio_device_node(iio_device_node);
     iio_fd = open(iio_device_node, O_RDONLY);
@@ -428,7 +483,6 @@ int MPLSensor::inv_constructor_default_enable()
         return result;
     }
 
-    // TODO: double-check for GED tablet
     // result = inv_enable_motion_no_motion();
     result = inv_enable_fast_nomot();
     if (result) {
@@ -452,8 +506,13 @@ int MPLSensor::inv_constructor_default_enable()
         if (result) {
             LOG_RESULT_LOCATION(result);
             return result;
+        } else {
+            mFeatureActiveMask |= INV_COMPASS_CAL;
         }
 
+        // specify MPL's trust weight, used by compass algorithms
+        inv_vector_compass_cal_sensitivity(3);
+
         result = inv_enable_compass_bias_w_gyro();
         if (result) {
             LOG_RESULT_LOCATION(result);
@@ -477,6 +536,9 @@ int MPLSensor::inv_constructor_default_enable()
     if (result) {
         LOG_RESULT_LOCATION(result);
         return result;
+    } else {
+        // 9x sensor fusion enables Compass fit
+        mFeatureActiveMask |= INV_COMPASS_FIT;
     }
 
     result = inv_enable_no_gyro_fusion();
@@ -485,7 +547,6 @@ int MPLSensor::inv_constructor_default_enable()
         return result;
     }
 
-    // TODO: double-check for GED tablet
     result = inv_enable_quat_accuracy_monitor();
     if (result) {
         LOG_RESULT_LOCATION(result);
@@ -513,15 +574,15 @@ void MPLSensor::inv_set_device_properties()
 
     /* accel setup */
     orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
-    // BMA250
-    //inv_set_accel_orientation_and_scale(orient, 1LL << 22);
-    // MPU6050
+    /* use for third party accel input subsystem driver
+    inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    */
     inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
 
     /* compass setup */
     signed char orientMtx[9];
     mCompassSensor->getOrientationMatrix(orientMtx);
-    orient = 
+    orient =
         inv_orientation_matrix_to_scalar(orientMtx);
     long sensitivity;
     sensitivity = mCompassSensor->getSensitivity();
@@ -533,13 +594,13 @@ void MPLSensor::loadDMP()
     int res, fd;
     FILE *fptr;
 
-    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) {
+    if (isMpu3050() == true) {
         //DMP support only for MPU6xxx/9xxx currently
         return;
     }
 
     /* load DMP firmware */
-    LOGV_IF(SYSFS_VERBOSE, 
+    LOGV_IF(SYSFS_VERBOSE,
             "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
     fd = open(mpu.firmware_loaded, O_RDONLY);
     if(fd < 0) {
@@ -572,13 +633,13 @@ void MPLSensor::inv_get_sensors_orientation()
     FILE *fptr;
 
     // get gyro orientation
-    LOGV_IF(SYSFS_VERBOSE, 
+    LOGV_IF(SYSFS_VERBOSE,
             "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
     fptr = fopen(mpu.gyro_orient, "r");
     if (fptr != NULL) {
         int om[9];
         fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
-               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
                &om[6], &om[7], &om[8]);
         fclose(fptr);
 
@@ -601,13 +662,13 @@ void MPLSensor::inv_get_sensors_orientation()
     }
 
     // get accel orientation
-    LOGV_IF(SYSFS_VERBOSE, 
+    LOGV_IF(SYSFS_VERBOSE,
             "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
     fptr = fopen(mpu.accel_orient, "r");
     if (fptr != NULL) {
         int om[9];
-        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
-               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 
+        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
                &om[6], &om[7], &om[8]);
         fclose(fptr);
 
@@ -637,7 +698,6 @@ MPLSensor::~MPLSensor()
     /* Close open fds */
     if (iio_fd > 0)
         close(iio_fd);
-
     if( accel_fd > 0 )
         close(accel_fd );
     if (gyro_temperature_fd > 0)
@@ -645,6 +705,10 @@ MPLSensor::~MPLSensor()
     if (sysfs_names_ptr)
         free(sysfs_names_ptr);
 
+    if (isDmpDisplayOrientationOn()) {
+        closeDmpOrientFd();
+    }
+
     /* Turn off Gyro master enable          */
     /* A workaround until driver handles it */
     /* TODO: Turn off and close all sensors */
@@ -665,18 +729,13 @@ MPLSensor::~MPLSensor()
 #endif
 }
 
-#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) // ID_GY = 0
-#define A_ENABLED  ((1 << ID_A)  & enabled_sensors) // ID_A  = 1
-#ifdef INVENSENSE_COMPASS_CAL                       // ID_M  = 2
-#define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
-#else
-// TODO: ID_M = 2 even for 3rd-party solution
+#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
+#define A_ENABLED  ((1 << ID_A)  & enabled_sensors)
 #define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
-#endif
-#define O_ENABLED  ((1 << ID_O)  & enabled_sensors) // ID_O  = 3
-#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4
-#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5
-#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6
+#define O_ENABLED  ((1 << ID_O)  & enabled_sensors)
+#define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
+#define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
+#define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
 
 /* TODO: this step is optional, remove?  */
 int MPLSensor::setGyroInitialState()
@@ -685,7 +744,7 @@ int MPLSensor::setGyroInitialState()
 
     int res = 0;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
             HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
     int fd = open(mpu.gyro_fifo_rate, O_RDWR);
     res = errno;
@@ -706,7 +765,7 @@ int MPLSensor::setGyroInitialState()
     return 0;
 }
 
-/* this applies to BMA250 only */
+/* this applies to BMA250 Input Subsystem Driver only */
 int MPLSensor::setAccelInitialState()
 {
     VFUNC_LOG;
@@ -737,7 +796,7 @@ int MPLSensor::onPower(int en)
     char buf[sizeof(int)+1];
     int count, curr_power_state;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
             en, mpu.power_state, getTimestamp());
     int tempFd = open(mpu.power_state, O_RDWR);
     res = errno;
@@ -754,16 +813,16 @@ int MPLSensor::onPower(int en)
         } else {
             sscanf(buf, "%d", &curr_power_state);
         }
-       
-        if (en!=curr_power_state) {    
+
+        if (en!=curr_power_state) {
             if((res=enable_sysfs_sensor(tempFd, en)) < 0) {
                 LOGE("HAL:Couldn't write power state");
             }
         } else {
             LOGV_IF(EXTRA_VERBOSE,
-                    "HAL:Power state already enable/disable curr=%d new=%d", 
+                    "HAL:Power state already enable/disable curr=%d new=%d",
                     curr_power_state, en);
-            close(tempFd);   
+            close(tempFd);
         }
     }
     return res;
@@ -782,24 +841,32 @@ int MPLSensor::onDMP(int en)
     //3. Either Gyro or Accel must be enabled/configured before next step
     //4. Enable DMP
 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+            mpu.firmware_loaded, getTimestamp());
     if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
         LOGE("HAL:ERR can't get firmware_loaded status");
     } else if (status == 1) {
         //Write only if curr DMP state <> request
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+                mpu.dmp_on, getTimestamp());
         if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
             LOGE("HAL:ERR can't read DMP state");
         } else if (status != en) {
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.dmp_on, getTimestamp());
             if (write_sysfs_int(mpu.dmp_on, en) < 0) {
-                LOGE("HAL:ERR can't write dmp_on");  
+                LOGE("HAL:ERR can't write dmp_on");
             } else {
-                res= 0;	//Indicate write successful
+                res = 0;	//Indicate write successful
             }
             //Enable DMP interrupt
+            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                    en, mpu.dmp_int_on, getTimestamp());
             if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
-                LOGE("HAL:ERR can't en/dis DMP interrupt");  
+                LOGE("HAL:ERR can't en/dis DMP interrupt");
             }
         } else {
-            res= 0;  	//DMP already set as requested
+            res= 0;	//DMP already set as requested
         }
     } else {
         LOGE("HAL:ERR No DMP image");
@@ -824,7 +891,7 @@ int MPLSensor::enableLPQuaternion(int en)
         mFeatureActiveMask &= ~INV_DMP_QUATERNION;
         LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
     } else {
-        if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {    
+        if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
             LOGE("HAL:ERR can't enable LP Quaternion");
         } else {
             mFeatureActiveMask |= INV_DMP_QUATERNION;
@@ -839,33 +906,65 @@ int MPLSensor::enableQuaternionData(int en)
     int res= 0;
     VFUNC_LOG;
 
-    //Enable DMP quaternion
+    // Enable DMP quaternion
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.quaternion_on, getTimestamp());
     if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
         LOGE("HAL:ERR can't write DMP quaternion_on");
-        res= -1;	//Indicate an err
-    } 
+        res = -1;	//Indicate an err
+    }
 
     if (!en) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.in_quat_r_en, getTimestamp());
+        if (write_sysfs_int(mpu.in_quat_r_en, 0) < 0) {
+            LOGE("HAL:ERR write in_quat_r_en");
+        }
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.in_quat_x_en, getTimestamp());
+        if (write_sysfs_int(mpu.in_quat_x_en, 0) < 0) {
+            LOGE("HAL:ERR write in_quat_x_en");
+        }
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.in_quat_y_en, getTimestamp());
+        if (write_sysfs_int(mpu.in_quat_y_en, 0) < 0) {
+            LOGE("HAL:ERR write in_quat_y_en");
+        }
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                0, mpu.in_quat_z_en, getTimestamp());
+
+        if (write_sysfs_int(mpu.in_quat_z_en, 0) < 0) {
+            LOGE("HAL:ERR write in_quat_z_en");
+        }
+
         LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
         inv_quaternion_sensor_was_turned_off();
     } else {
         LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                1, mpu.in_quat_r_en, getTimestamp());
         if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) {
             LOGE("HAL:ERR write in_quat_r_en");
         }
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                1, mpu.in_quat_x_en, getTimestamp());
         if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) {
             LOGE("HAL:ERR write in_quat_x_en");
         }
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                1, mpu.in_quat_y_en, getTimestamp());
         if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) {
             LOGE("HAL:ERR write in_quat_y_en");
         }
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                1, mpu.in_quat_z_en, getTimestamp());
         if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) {
-            LOGE("HAL:ERR write in_quat_z_en"); 
+            LOGE("HAL:ERR write in_quat_z_en");
         }
     }
 
     return res;
-
 }
 
 int MPLSensor::enableTap(int en)
@@ -895,7 +994,7 @@ int MPLSensor::masterEnable(int en)
 
     int res = 0;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
             en, mpu.chip_enable, getTimestamp());
     int tempFd = open(mpu.chip_enable, O_RDWR);
     res = errno;
@@ -915,7 +1014,7 @@ int MPLSensor::enableGyro(int en)
     int res = 0;
 
     /* need to also turn on/off the master enable */
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
             en, mpu.gyro_enable, getTimestamp());
     int tempFd = open(mpu.gyro_enable, O_RDWR);
     res = errno;
@@ -930,7 +1029,7 @@ int MPLSensor::enableGyro(int en)
         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
         inv_gyro_was_turned_off();
     } else {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, mpu.gyro_x_fifo_enable, getTimestamp());
         tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR);
         res = errno;
@@ -941,7 +1040,7 @@ int MPLSensor::enableGyro(int en)
                  mpu.gyro_x_fifo_enable, strerror(res), res);
         }
 
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, mpu.gyro_y_fifo_enable, getTimestamp());
         tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR);
         res = errno;
@@ -952,7 +1051,7 @@ int MPLSensor::enableGyro(int en)
                  mpu.gyro_y_fifo_enable, strerror(res), res);
         }
 
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, mpu.gyro_z_fifo_enable, getTimestamp());
         tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR);
         res = errno;
@@ -973,23 +1072,23 @@ int MPLSensor::enableAccel(int en)
 
     int res;
 
-   /* need to also turn on/off the master enable */
-   LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
-           en, mpu.accel_enable, getTimestamp());
-   int tempFd = open(mpu.accel_enable, O_RDWR);
-   res = errno;
-   if (tempFd > 0) {
-       res = enable_sysfs_sensor(tempFd, en);
-   } else {
-       LOGE("HAL:open of %s failed with '%s' (%d)",
-            mpu.accel_enable, strerror(res), res);
-   }
+    /* need to also turn on/off the master enable */
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            en, mpu.accel_enable, getTimestamp());
+    int tempFd = open(mpu.accel_enable, O_RDWR);
+    res = errno;
+    if (tempFd > 0) {
+        res = enable_sysfs_sensor(tempFd, en);
+    } else {
+        LOGE("HAL:open of %s failed with '%s' (%d)",
+             mpu.accel_enable, strerror(res), res);
+    }
 
     if (!en) {
         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
         inv_accel_was_turned_off();
     } else {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, mpu.accel_x_fifo_enable, getTimestamp());
         tempFd = open(mpu.accel_x_fifo_enable, O_RDWR);
         res = errno;
@@ -1000,7 +1099,7 @@ int MPLSensor::enableAccel(int en)
                  mpu.accel_x_fifo_enable, strerror(res), res);
         }
 
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, mpu.accel_y_fifo_enable, getTimestamp());
         tempFd = open(mpu.accel_y_fifo_enable, O_RDWR);
         res = errno;
@@ -1011,7 +1110,7 @@ int MPLSensor::enableAccel(int en)
                  mpu.accel_y_fifo_enable, strerror(res), res);
         }
 
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
                 en, mpu.accel_z_fifo_enable, getTimestamp());
         tempFd = open(mpu.accel_z_fifo_enable, O_RDWR);
         res = errno;
@@ -1024,10 +1123,10 @@ int MPLSensor::enableAccel(int en)
     }
 
     if (!en && USE_THIRD_PARTY_ACCEL == 0) {
-    } 
+    }
 
     if(USE_THIRD_PARTY_ACCEL == 1 && en) {
-        setAccelInitialState(); // BMA250
+        //setAccelInitialState();// BMA250
     }
     return res;
 }
@@ -1040,7 +1139,7 @@ int MPLSensor::enableCompass(int en)
     if (en == 0) {
         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
         inv_compass_was_turned_off();
-    } 
+    }
     return res;
 }
 
@@ -1095,18 +1194,20 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
     inv_error_t res = -1;
     int on = 1;
     int off = 0;
+    int cal_stored = 0;
 
     // Sequence to enable or disable a sensor
-    // 1. enable Power state 
+    // 1. enable Power state
     // 2. reset master enable (=0)
     // 3. enable or disable a sensor
     // 4. set master enable (=1)
 
-    if (changed & ((1 << Gyro) | (1 << Accelerometer) |
-            (mCompassSensor->isIntegrated() << MagneticField))) {
+    if (isLowPowerQuatEnabled() ||
+        changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+        (mCompassSensor->isIntegrated() << MagneticField))) {
         /* ensure power state is on */
         onPower(1);
-      
+
         /* reset master enable */
         res = masterEnable(0);
         if(res < 0) {
@@ -1116,35 +1217,45 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
 
     LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
 
-    if (changed & (1 << Gyro)) {
-        if(sensors & INV_THREE_AXIS_GYRO) {
+    if (changed & ((1 << Gyro) | (1 << RawGyro))) {
+        if (sensors & INV_THREE_AXIS_GYRO) {
             LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro");
             res = enableGyro(on);
             if(res < 0) {
                 return res;
             }
-        } else if((sensors & INV_THREE_AXIS_GYRO) == 0) {
+        } else if ((sensors & INV_THREE_AXIS_GYRO) == 0) {
             LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
             res = enableGyro(off);
             if(res < 0) {
                 return res;
             }
+
+            if (!cal_stored) {
+                storeCalibration();
+                cal_stored = 1;
+            }
         }
     }
 
     if (changed & (1 << Accelerometer)) {
-        if(sensors & INV_THREE_AXIS_ACCEL) {
+        if (sensors & INV_THREE_AXIS_ACCEL) {
             LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel");
             res = enableAccel(on);
             if(res < 0) {
                 return res;
             }
-        } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+        } else if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
             LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
             res = enableAccel(off);
             if(res < 0) {
                 return res;
             }
+
+            if (!cal_stored) {
+                storeCalibration();
+                cal_stored = 1;
+            }
         }
     }
 
@@ -1162,51 +1273,121 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
             if(res < 0) {
                 return res;
             }
+
+            if (!cal_stored) {
+                storeCalibration();
+                cal_stored = 1;
+            }
         }
     }
 
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
-    // Enable LP Quat
-    if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
-            (1 << LinearAccel) | (1 << Gravity)))) {
-        if (!checkLPQuaternion()) {
-            enableLPQuaternion(1);
-        } else {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+    if ( isLowPowerQuatEnabled() ) {
+        // Enable LP Quat
+        if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+                (1 << LinearAccel) | (1 << Gravity)))) {
+            if (!(changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+                    (mCompassSensor->isIntegrated() << MagneticField)))) {
+                /* ensure power state is on */
+                onPower(1);
+                /* reset master enable */
+                res = masterEnable(0);
+                if(res < 0) {
+                    return res;
+                }
+            }
+            if (!checkLPQuaternion()) {
+                enableLPQuaternion(1);
+            } else {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+            }
+        } else if (checkLPQuaternion()) {
+            enableLPQuaternion(0);
         }
-    } else if (checkLPQuaternion()) {
-        enableLPQuaternion(0);
     }
-#endif
 
-    /*
-        if sensor & THREE_AXIS_GYRO
-            enable = 1
-        if sensor & THREE_AXIS_ACCEL
-            enable = 1
-        if compass_on_secondary
-            if sensor & THREE_AXIS_COMPASS
-                enable = 1
-        else
-            enable = 0
-    */
-    if (changed & ((1 << Gyro) | (1 << Accelerometer) |
+    if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
             (mCompassSensor->isIntegrated() << MagneticField))) {
-        if (sensors & 
-            (INV_THREE_AXIS_GYRO 
-                | INV_THREE_AXIS_ACCEL 
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+            if ( isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
+                // disable DMP event interrupt only (w/ data interrupt)
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        0, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't disable DMP event interrupt");
+                    return res;
+                }
+            }
+
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                // enable DMP
+                onDMP(1);
+
+                res = enableAccel(on);
+                if(res < 0) {
+                    return res;
+                }
+
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    return res;
+                }
+            }
+
             res = masterEnable(1);
             if(res < 0) {
                 return res;
             }
         } else { // all sensors idle -> reduce power
-            res = onPower(0);
-            if(res < 0) {
-                return res;
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                // enable DMP
+                onDMP(1);
+                // enable DMP event interrupt only (no data interrupt)
+                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                        1, mpu.dmp_event_int_on, getTimestamp());
+                if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
+                    res = -1;
+                    LOGE("HAL:ERR can't enable DMP event interrupt");
+                }
+                res = enableAccel(on);
+                if(res < 0) {
+                    return res;
+                }
+                if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+                    res = turnOffAccelFifo();
+                }
+                if(res < 0) {
+                    return res;
+                }
+                res = masterEnable(1);
+                if(res < 0) {
+                    return res;
+                }
             }
+            else {
+                res = onPower(0);
+                if(res < 0) {
+                    return res;
+                }
+            }
+
+            if (!cal_stored) {
+                storeCalibration();
+                cal_stored = 1;
+            }
+        }
+    } else if ((changed & ((!mCompassSensor->isIntegrated()) << MagneticField)) &&
+            !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
+        if (!cal_stored) {
             storeCalibration();
+            cal_stored = 1;
         }
     }
 
@@ -1216,7 +1397,7 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
 /* Store calibration file */
 void MPLSensor::storeCalibration()
 {
-    if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) {
+    if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
        int res = inv_store_calibration();
        if (res) {
            LOGE("HAL:Cannot store calibration on file");
@@ -1244,6 +1425,16 @@ int MPLSensor::gyroHandler(sensors_event_t* s)
     return update;
 }
 
+int MPLSensor::rawGyroHandler(sensors_event_t* s)
+{
+    VHANDLER_LOG;
+    int update;
+    update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &s->gyro.status, &s->timestamp);
+    LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
+            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+    return update;
+}
+
 int MPLSensor::accelHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
@@ -1251,7 +1442,7 @@ int MPLSensor::accelHandler(sensors_event_t* s)
     update = inv_get_sensor_type_accelerometer(
         s->acceleration.v, &s->acceleration.status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
-            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], 
+            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
             s->timestamp, update);
     mAccelAccuracy = s->acceleration.status;
     return update;
@@ -1265,6 +1456,7 @@ int MPLSensor::compassHandler(sensors_event_t* s)
         s->magnetic.v, &s->magnetic.status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
             s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
+    mCompassAccuracy = s->magnetic.status;
     return update;
 }
 
@@ -1317,9 +1509,17 @@ int MPLSensor::enable(int32_t handle, int en)
     VFUNC_LOG;
 
     android::String8 sname;
-    int what = -1;
+    int what = -1, err = 0;
 
     switch (handle) {
+    case ID_SO:
+        sname = "Screen Orientation";
+        LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
+            (mDmpOrientationEnabled? "en": "dis"),
+            (en? "en" : "dis"));
+        enableDmpOrientation(en && isDmpDisplayOrientationOn());
+        mDmpOrientationEnabled = en;
+        return 0;
     case ID_A:
         what = Accelerometer;
         sname = "Accelerometer";
@@ -1336,6 +1536,10 @@ int MPLSensor::enable(int32_t handle, int en)
         what = Gyro;
         sname = "Gyro";
         break;
+    case ID_RG:
+        what = RawGyro;
+        sname = "RawGyro";
+        break;
     case ID_GR:
         what = Gravity;
         sname = "Gravity";
@@ -1354,11 +1558,10 @@ int MPLSensor::enable(int32_t handle, int en)
         break;
     }
 
-    if (uint32_t(what) >= numSensors)
+    if (uint32_t(what) >= NUMSENSORS)
         return -EINVAL;
 
     int newState = en ? 1 : 0;
-    int err = 0;
     unsigned long sen_mask;
 
     LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
@@ -1367,7 +1570,6 @@ int MPLSensor::enable(int32_t handle, int en)
     LOGV_IF(PROCESS_VERBOSE,
             "HAL:%s sensor state change what=%d", sname.string(), what);
 
-    // TODO: disabled for GED tablet
     // pthread_mutex_lock(&mMplMutex);
     // pthread_mutex_lock(&mHALMutex);
 
@@ -1389,6 +1591,7 @@ int MPLSensor::enable(int32_t handle, int en)
 
         switch (what) {
             case Gyro:
+            case RawGyro:
             case Accelerometer:
             case MagneticField:
                 if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
@@ -1422,7 +1625,7 @@ int MPLSensor::enable(int32_t handle, int en)
     // pthread_mutex_unlock(&mHALMutex);
 
 #ifdef INV_PLAYBACK_DBG
-    /* apparently the logging needs to be go through this sequence 
+    /* apparently the logging needs to be go through this sequence
        to properly flush the log file */
     inv_turn_off_data_logging();
     fclose(logfile);
@@ -1442,41 +1645,46 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
     int what = -1;
 
     switch (handle) {
-    case ID_A:
-        what = Accelerometer;
-        sname = "Accelerometer";
-        break;
-    case ID_M:
-        what = MagneticField;
-        sname = "MagneticField";
-        break;
-    case ID_O:
-        what = Orientation;
-        sname = "Orientation";
-        break;
-    case ID_GY:
-        what = Gyro;
-        sname = "Gyro";
-        break;
-    case ID_GR:
-        what = Gravity;
-        sname = "Gravity";
-        break;
-    case ID_RV:
-        what = RotationVector;
-        sname = "RotationVector";
-        break;
-    case ID_LA:
-        what = LinearAccel;
-        sname = "LinearAccel";
-        break;
-    default: // this takes care of all the gestures
-        what = handle;
-        sname = "Others";
-        break;
+        case ID_SO:
+            return update_delay();
+        case ID_A:
+            what = Accelerometer;
+            sname = "Accelerometer";
+            break;
+        case ID_M:
+            what = MagneticField;
+            sname = "MagneticField";
+            break;
+        case ID_O:
+            what = Orientation;
+            sname = "Orientation";
+            break;
+        case ID_GY:
+            what = Gyro;
+            sname = "Gyro";
+            break;
+        case ID_RG:
+            what = RawGyro;
+            sname = "RawGyro";
+            break;
+        case ID_GR:
+            what = Gravity;
+            sname = "Gravity";
+            break;
+        case ID_RV:
+            what = RotationVector;
+            sname = "RotationVector";
+            break;
+        case ID_LA:
+            what = LinearAccel;
+            sname = "LinearAccel";
+            break;
+        default: // this takes care of all the gestures
+            what = handle;
+            sname = "Others";
+            break;
     }
 
-// TODO: disabled for GED tablet
 #if 0
     // skip the 1st call for enalbing sensors called by ICS/JB sensor service
     static int counter_delay = 0;
@@ -1492,7 +1700,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
     }
 #endif
 
-    if (uint32_t(what) >= numSensors)
+    if (uint32_t(what) >= NUMSENSORS)
         return -EINVAL;
 
     if (ns < 0)
@@ -1500,7 +1708,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
 
     LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
 
-    // TODO: for GED tablet
     // limit all rates to reasonable ones */
 /*
     if (ns < 10000000LL) {
@@ -1516,6 +1723,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
 
     switch (what) {
         case Gyro:
+        case RawGyro:
         case Accelerometer:
             for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
                     i++) {
@@ -1529,6 +1737,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
         case MagneticField:
             if (mCompassSensor->isIntegrated() &&
                     (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+                    ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
                     ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
                  LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
                  return 0;
@@ -1539,7 +1748,12 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
         case RotationVector:
         case LinearAccel:
         case Gravity:
-            for (int i = 0; i < numSensors; i++) {
+            if ( isLowPowerQuatEnabled() ) {
+                LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
+                break;
+            }
+
+            for (int i = 0; i < NUMSENSORS; i++) {
                 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
                     LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
                     return 0;
@@ -1548,7 +1762,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
             break;
     }
 
-    // TODO: disabled for GED tablet
     // pthread_mutex_lock(&mHALMutex);
     int res = update_delay();
     // pthread_mutex_unlock(&mHALMutex);
@@ -1562,33 +1775,31 @@ int MPLSensor::update_delay() {
     int64_t got;
 
     if (mEnabled) {
-        uint64_t wanted = -1LLU;
-        uint64_t wanted_ec = -1LLU;
+        int64_t wanted = 1000000000;
+        int64_t wanted_3rd_party_sensor = 1000000000;
 
-        // Sequence to change sensor's FIFO rate 
-        // 1. enable Power state 
+        // Sequence to change sensor's FIFO rate
+        // 1. enable Power state
         // 2. reset master enable
         // 3. Update delay
         // 4. set master enable
 
-        // TODO: unnecessary for IIO
         // ensure power on
-        // onPower(1);
+        onPower(1);
 
-        // TODO: unnecessary for IIO
         // reset master enable
-        // masterEnable(0);
+        masterEnable(0);
 
         /* search the minimum delay requested across all enabled sensors */
-        for (int i = 0; i < numSensors; i++) {
+        for (int i = 0; i < NUMSENSORS; i++) {
             if (mEnabled & (1 << i)) {
-                uint64_t ns = mDelays[i];
+                int64_t ns = mDelays[i];
                 wanted = wanted < ns ? wanted : ns;
             }
         }
 
-	      // same delay for ext compass
-        wanted_ec = wanted;
+        // same delay for 3rd party Accel or Compass
+        wanted_3rd_party_sensor = wanted;
 
         /* mpl rate in us in future maybe different for
            gyro vs compass vs accel */
@@ -1607,25 +1818,6 @@ int MPLSensor::update_delay() {
         inv_set_accel_sample_rate(mplAccelRate);
         inv_set_compass_sample_rate(mplCompassRate);
 
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
-        // Set LP Quaternion sample rate if enabled
-        if (checkLPQuaternion()) {
-            // TODO: need to verify this part for LPQ
-            if (wanted < 5000000LL) {
-                enableLPQuaternion(0);
-            } else {
-                inv_set_quat_sample_rate(rateInus);
-                // set DMP output rate to FIFO 
-                write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted);
-
-                //DMP running rate must be @ 200Hz 
-                wanted= 5000000LL;	
-                LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus);
-            }
-        }
-#endif
-
         /* TODO: Test 200Hz */
         // inv_set_gyro_sample_rate(5000);
         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
@@ -1634,12 +1826,30 @@ int MPLSensor::update_delay() {
 
         int enabled_sensors = mEnabled;
         int tempFd = -1;
-        if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
-            uint64_t tempRate = wanted;
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            if (isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
+                bool setDMPrate= 0;
+                // Set LP Quaternion sample rate if enabled
+                if (checkLPQuaternion()) {
+                    if (wanted < RATE_200HZ) {
+                        enableLPQuaternion(0);
+                    } else {
+                        inv_set_quat_sample_rate(rateInus);
+                        setDMPrate= 1;
+                    }
+                }
+
+                if (checkDMPOrientation() || setDMPrate==1) {
+                    getDmpRate(&wanted);
+                }
+            }
+
+            int64_t tempRate = wanted;
             LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
             //nsToHz
             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
-                    1000000000.f / tempRate, mpu.gyro_fifo_rate, 
+                    1000000000.f / tempRate, mpu.gyro_fifo_rate,
                     getTimestamp());
             tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
             res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
@@ -1650,15 +1860,16 @@ int MPLSensor::update_delay() {
             //nsToHz (BMA250)
             if(USE_THIRD_PARTY_ACCEL == 1) {
                 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
-                        wanted / 1000000L, mpu.accel_fifo_rate, 
+                        wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
                         getTimestamp());
                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
-                res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
             }
 
             if (!mCompassSensor->isIntegrated()) {
-                mCompassSensor->setDelay(ID_M, wanted_ec);
+                LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
+                mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
                 got = mCompassSensor->getDelay(ID_M);
                 inv_set_compass_sample_rate(got / 1000);
             }
@@ -1679,7 +1890,14 @@ int MPLSensor::update_delay() {
         } else {
 
             if (GY_ENABLED) {
-                wanted = mDelays[Gyro];
+                wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
+                    (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
+                    (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
+
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                    getDmpRate(&wanted);
+                }
+
                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
                         1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
                 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
@@ -1690,17 +1908,30 @@ int MPLSensor::update_delay() {
             if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
                 if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
                     wanted = mDelays[Gyro];
+                }
+                else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
+                    wanted = mDelays[RawGyro];
+
                 } else {
                     wanted = mDelays[Accelerometer];
                 }
+
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                    getDmpRate(&wanted);
+                }
+
                 /* TODO: use function pointers to calculate delay value specific to vendor */
                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
                         1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
-                //BMA250 in ms
-                //res = write_attribute_sensor(tempFd, wanted / 1000000L);
-                //MPU6050 in hz
-                res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                if(USE_THIRD_PARTY_ACCEL == 1) {
+                    //BMA250 in ms
+                    res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                }
+                else {
+                    //MPUxxxx in hz
+                    res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                }
                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
             }
 
@@ -1711,12 +1942,20 @@ int MPLSensor::update_delay() {
                 } else {
                     if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
                         wanted = mDelays[Gyro];
-                    } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
+                    }
+                    else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
+                        wanted = mDelays[RawGyro];
+                    } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
                         wanted = mDelays[Accelerometer];
                     } else {
                         wanted = mDelays[MagneticField];
                     }
+
+                    if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
+                        getDmpRate(&wanted);
+                    }
                 }
+
                 mCompassSensor->setDelay(ID_M, wanted);
                 got = mCompassSensor->getDelay(ID_M);
                 inv_set_compass_sample_rate(got / 1000);
@@ -1724,44 +1963,27 @@ int MPLSensor::update_delay() {
 
         }
 
-        /*
-            if sensor & THREE_AXIS_GYRO
-                enable = 1
-            if sensor & THREE_AXIS_ACCEL
-                enable = 1
-            if compass_on_secondary
-                if sensor & THREE_AXIS_COMPASS
-                    enable = 1
-            else
-                enable = 0
-        */
         unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
-        if (sensors & 
-            (INV_THREE_AXIS_GYRO 
-                | INV_THREE_AXIS_ACCEL 
+        if (sensors &
+            (INV_THREE_AXIS_GYRO
+                | INV_THREE_AXIS_ACCEL
                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
-// TODO: unnecessary for IIO
-#if 0
             res = masterEnable(1);
             if(res < 0) {
                 return res;
             }
-#endif
         } else { // all sensors idle -> reduce power
-// TODO: unnecessary for IIO
-#if 0
             res = onPower(0);
             if(res < 0) {
                 return res;
             }
-#endif
         }
     }
 
     return res;
 }
 
-/* use for third party accel */
+/* For Third Party Accel Input Subsystem Drivers only */
 int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
 {
     VHANDLER_LOG;
@@ -1804,15 +2026,12 @@ int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
         mAccelInputReader.next();
     }
 
-    LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived);
-
     return numEventReceived;
 }
 
-/** 
+/**
  *  Should be called after reading at least one of gyro
- *  compass or accel data. You should only read 1 sample of
- *  data and call this.
+ *  compass or accel data. (Also okay for handling all of them).
  *  @returns 0, if successful, error number if not.
  */
 int MPLSensor::executeOnData(sensors_event_t* data, int count)
@@ -1841,7 +2060,7 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count)
     }
 
     // load up virtual sensors
-    for (int i = 0; i < numSensors; i++) {
+    for (int i = 0; i < NUMSENSORS; i++) {
         int update;
         if (mEnabled & (1 << i)) {
             update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
@@ -1858,10 +2077,11 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count)
     return numEventReceived;
 }
 
+// collect data for MPL (but NOT sensor service currently), from driver layer
 int MPLSensor::readEvents(sensors_event_t *data, int count) {
 
 
-    int lp_quaternion_on, nbyte;
+    int lp_quaternion_on = 0, nbyte;
     int i, nb, mask = 0, numEventReceived = 0,
         sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1: 0) +
@@ -1870,24 +2090,23 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
 
     nbyte= (8 * sensors + 8) * 1;
 
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
-    lp_quaternion_on = checkLPQuaternion();
-    if (lp_quaternion_on == 1) {
-        nbyte += sizeof(mCachedQuaternionData);		//currently 16 bytes for Q data
+    if (isLowPowerQuatEnabled()) {
+        lp_quaternion_on = checkLPQuaternion();
+        if (lp_quaternion_on == 1) {
+            nbyte += sizeof(mCachedQuaternionData);		//currently 16 bytes for Q data
+        }
     }
-#endif
 
-    // TODO: disabled for GED tablet
     // pthread_mutex_lock(&mMplMutex);
     // pthread_mutex_lock(&mHALMutex);
 
-    size_t rsize = read(iio_fd, rdata, nbyte);
+    ssize_t rsize = read(iio_fd, rdata, nbyte);
     if (sensors == 0) {
         // read(iio_fd, rdata, nbyte);
-        read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH);
+        read(iio_fd, rdata, IIO_BUFFER_LENGTH);
     }
-/*
+
+#ifdef TESTING
     LOGI("get one sample of IIO data with size: %d", rsize);
     LOGI("sensors: %d", sensors);
 
@@ -1907,27 +2126,20 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
         *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
-*/
+#endif
 
-    // TODO: need to verify this for LPQ
     if (rsize < (nbyte - 8)) {
         LOGE("HAL:ERR Full data packet was not read");
         // return -1;
     }
 
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
-    if (lp_quaternion_on == 1) {
+    if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
+
         for (i=0; i< 4; i++) {
             mCachedQuaternionData[i]= *(long*)rdata;
             rdata += sizeof(long);
         }
     }
-/*
-    LOGV("HAL:rdata= %x sensors= %d  lp_q_on= %d nbyte= %d rsize= %d",
-        rdata, sensors, lp_quaternion_on, nbyte, rsize);  //tbd
-*/
-#endif
 
     for (i = 0; i < 3; i++) {
         if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
@@ -1942,11 +2154,11 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
         }
     }
 
-    mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
-        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0));
+    mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
+        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
     if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
             (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
-        mask |= 4;
+        mask |= 1 << MagneticField;
     }
 
     mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
@@ -1954,58 +2166,61 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
         mCompassTimestamp = mSensorTimestamp;
     }
 
-    // send down temperature every 0.5 seconds
-    if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
-        mTempCurrentTime = mSensorTimestamp;
-        long long temperature[2];
-        if(inv_read_temperature(temperature) == 0) {
-            LOGV_IF(INPUT_DATA,
-                    "HAL:inv_read_temperature = %lld, timestamp= %lld",
-                    temperature[0], temperature[1]);
-            inv_build_temp(temperature[0], temperature[1]);
-        }
+    if (mask & (1 << Gyro)) {
+        // send down temperature every 0.5 seconds
+        // with timestamp measured in "driver" layer
+        if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+            mTempCurrentTime = mSensorTimestamp;
+            long long temperature[2];
+            if(inv_read_temperature(temperature) == 0) {
+                LOGV_IF(INPUT_DATA,
+                        "HAL:inv_read_temperature = %lld, timestamp= %lld",
+                        temperature[0], temperature[1]);
+                inv_build_temp(temperature[0], temperature[1]);
+            }
 #ifdef TESTING
-        long bias[3], temp, temp_slope[3];
-        inv_get_gyro_bias(bias, &temp);
-        inv_get_gyro_ts(temp_slope);
-        
-        LOGI("T: %.3f "
-             "GB: %+13f %+13f %+13f "
-             "TS: %+13f %+13f %+13f "
-             "\n", 
-             (float)temperature[0] / 65536.f,
-             (float)bias[0] / 65536.f / 16.384f,
-             (float)bias[1] / 65536.f / 16.384f,
-             (float)bias[2] / 65536.f / 16.384f,
-             temp_slope[0] / 65536.f,
-             temp_slope[1] / 65536.f,
-             temp_slope[2] / 65536.f);
+            long bias[3], temp, temp_slope[3];
+            inv_get_gyro_bias(bias, &temp);
+            inv_get_gyro_ts(temp_slope);
+
+            LOGI("T: %.3f "
+                 "GB: %+13f %+13f %+13f "
+                 "TS: %+13f %+13f %+13f "
+                 "\n",
+                 (float)temperature[0] / 65536.f,
+                 (float)bias[0] / 65536.f / 16.384f,
+                 (float)bias[1] / 65536.f / 16.384f,
+                 (float)bias[2] / 65536.f / 16.384f,
+                 temp_slope[0] / 65536.f,
+                 temp_slope[1] / 65536.f,
+                 temp_slope[2] / 65536.f);
 #endif
-    }
+        }
 
-    if (mask & 1) {
         mPendingMask |= 1 << Gyro;
+        mPendingMask |= 1 << RawGyro;
+
         if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
             inv_build_gyro(mCachedGyroData, mSensorTimestamp);
             LOGV_IF(INPUT_DATA,
                     "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
-                    mCachedGyroData[0], mCachedGyroData[1], 
+                    mCachedGyroData[0], mCachedGyroData[1],
                     mCachedGyroData[2], mSensorTimestamp);
         }
     }
 
-    if (mask & 2) {
+    if (mask & (1 << Accelerometer)) {
         mPendingMask |= 1 << Accelerometer;
         if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
             inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
              LOGV_IF(INPUT_DATA,
                     "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
-                    mCachedAccelData[0], mCachedAccelData[1], 
+                    mCachedAccelData[0], mCachedAccelData[1],
                     mCachedAccelData[2], mSensorTimestamp);
         }
     }
 
-    if ((mask & 4) && mCompassSensor->isIntegrated()) {
+    if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
         int status = 0;
         if (mCompassSensor->providesCalibration()) {
             status = mCompassSensor->getAccuracy();
@@ -2014,21 +2229,19 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
         if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
             inv_build_compass(mCachedCompassData, status,
                               mCompassTimestamp);
-            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", 
-                    mCachedCompassData[0], mCachedCompassData[1], 
+            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
                     mCachedCompassData[2], mCompassTimestamp);
         }
     }
 
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
-    if (lp_quaternion_on == 1) {
+    if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
+
         inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
-        LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld - %d",
-                    mCachedQuaternionData[0], mCachedQuaternionData[1], 
+        LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
+                    mCachedQuaternionData[0], mCachedQuaternionData[1],
                     mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
     }
-#endif
 
     // pthread_mutex_unlock(&mMplMutex);
     // pthread_mutex_unlock(&mHALMutex);
@@ -2041,19 +2254,20 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
 {
     VHANDLER_LOG;
 
-    if (count < 1)
-        return -EINVAL;
-
     int numEventReceived = 0;
     int done = 0;
     int nb;
 
-    // TODO: disabled for GED tablet
-    // TODO: for AMI306
     // pthread_mutex_lock(&mMplMutex);
     // pthread_mutex_lock(&mHALMutex);
 
     done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+#ifdef COMPASS_YAS53x
+    if (mCompassSensor->checkCoilsReset() == 1) {
+       //Reset relevant compass settings
+       resetCompass();
+    }
+#endif
     if (done > 0) {
         int status = 0;
         if (mCompassSensor->providesCalibration()) {
@@ -2063,8 +2277,8 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
         if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
             inv_build_compass(mCachedCompassData, status,
                               mCompassTimestamp);
-            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", 
-                    mCachedCompassData[0], mCachedCompassData[1], 
+            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                    mCachedCompassData[0], mCachedCompassData[1],
                     mCachedCompassData[2], mCompassTimestamp);
         }
     }
@@ -2075,6 +2289,27 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
     return numEventReceived;
 }
 
+#ifdef COMPASS_YAS53x
+int MPLSensor::resetCompass()
+{
+    VFUNC_LOG;
+
+    //Reset compass cal if enabled
+    if (mFeatureActiveMask & INV_COMPASS_CAL) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
+       inv_init_vector_compass_cal();
+    }
+
+    //Reset compass fit if enabled
+    if (mFeatureActiveMask & INV_COMPASS_FIT) {
+       LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
+       inv_init_compass_fit();
+    }
+
+    return 0;
+}
+#endif
+
 int MPLSensor::getFd() const
 {
     VFUNC_LOG;
@@ -2097,6 +2332,206 @@ int MPLSensor::getCompassFd() const
     return fd;
 }
 
+int MPLSensor::turnOffAccelFifo() {
+        int i, res, tempFd;
+        char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
+            mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
+
+    for (i = 0; i < 3; i++) {
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            0, accel_fifo_enable[i], getTimestamp());
+        tempFd = open(accel_fifo_enable[i], O_RDWR);
+        res = errno;
+        if (tempFd > 0) {
+            res = enable_sysfs_sensor(tempFd, 0);
+            if (res < 0) {
+                return res;
+            }
+        } else {
+            LOGE("HAL:open of %s failed with '%s' (%d)",
+                accel_fifo_enable[i], strerror(res), res);
+            return res;
+        }
+    }
+
+    return res;
+}
+
+int MPLSensor::enableDmpOrientation(int en)
+{
+    VFUNC_LOG;
+    int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if ( isMpu3050() )
+        return res;
+
+    // on power if not already On
+    res = onPower(1);
+    // reset master enable
+    res = masterEnable(0);
+
+    if (en == 1) {
+        //Enable DMP orientation
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                en, mpu.display_orientation_on, getTimestamp());
+        if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+            LOGE("HAL:ERR can't enable Android orientation");
+            res = -1;	// indicate an err
+        }
+
+        // open DMP Orient Fd
+        res = openDmpOrientFd();
+
+        // enable DMP
+        res = onDMP(1);
+
+        // default DMP output rate to FIFO
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                5, mpu.dmp_output_rate, getTimestamp());
+        if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
+            LOGE("HAL:ERR can't default DMP output rate");
+        }
+
+        // set DMP rate to 200Hz
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                200, mpu.accel_fifo_rate, getTimestamp());
+        if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+            res = -1;
+            LOGE("HAL:ERR can't set DMP rate to 200Hz");
+        }
+
+        // enable accel engine
+        res = enableAccel(1);
+
+        // disable accel FIFO
+        if (!A_ENABLED) {
+            res = turnOffAccelFifo();
+        }
+
+        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
+    } else {
+        // disable DMP
+        res = onDMP(0);
+        // disable accel engine
+        if (!A_ENABLED) {
+            res = enableAccel(0);
+        }
+    }
+
+    res = masterEnable(1);
+    return res;
+}
+
+int MPLSensor::openDmpOrientFd()
+{
+    VFUNC_LOG;
+
+    if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
+        return -1;
+    }
+
+    dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
+    if (dmp_orient_fd < 0) {
+        LOGE("HAL:ERR couldn't open dmpOrient node");
+        return -1;
+    } else {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
+        return 0;
+    }
+}
+
+int MPLSensor::closeDmpOrientFd()
+{
+    VFUNC_LOG;
+    if (dmp_orient_fd >= 0)
+        close(dmp_orient_fd);
+    return 0;
+}
+
+int MPLSensor::dmpOrientHandler(int orient)
+{
+    VFUNC_LOG;
+
+    LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
+    return 0;
+}
+
+int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
+    VFUNC_LOG;
+
+    char dummy[4];
+    int screen_orientation = 0;
+    FILE *fp;
+
+    fp = fopen(mpu.event_display_orientation, "r");
+    if (fp == NULL) {
+        LOGE("HAL:cannot open event_display_orientation");
+        return 0;
+    }
+    fscanf(fp, "%d\n", &screen_orientation);
+    fclose(fp);
+
+    int numEventReceived = 0;
+
+    if (mDmpOrientationEnabled && count > 0) {
+        sensors_event_t temp;
+
+        temp.version = sizeof(sensors_event_t);
+        temp.sensor = ID_SO;
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+        temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
+        temp.screen_orientation = screen_orientation;
+#endif
+        struct timespec ts;
+        clock_gettime(CLOCK_MONOTONIC, &ts);
+        temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+        *data++ = temp;
+        count--;
+        numEventReceived++;
+    }
+
+    // read dummy data per driver's request
+    dmpOrientHandler(screen_orientation);
+    read(dmp_orient_fd, dummy, 4);
+
+    return numEventReceived;
+}
+
+int MPLSensor::getDmpOrientFd()
+{
+    VFUNC_LOG;
+
+    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
+    return dmp_orient_fd;
+
+}
+
+int MPLSensor::checkDMPOrientation()
+{
+    VFUNC_LOG;
+    return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
+}
+
+int MPLSensor::getDmpRate(int64_t *wanted)
+{
+    if (checkDMPOrientation() || checkLPQuaternion()) {
+        // set DMP output rate to FIFO
+        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+                int(1000000000.f / *wanted), mpu.dmp_output_rate,
+                getTimestamp());
+        write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
+
+        //DMP running rate must be @ 200Hz
+        *wanted= RATE_200HZ;
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
+    }
+    return 0;
+}
+
 int MPLSensor::getPollTime()
 {
     VHANDLER_LOG;
@@ -2194,7 +2629,7 @@ int MPLSensor::inv_read_temperature(long long *data)
         return -1;
     }
 
-    LOGV_IF(ENG_VERBOSE, 
+    LOGV_IF(ENG_VERBOSE,
             "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
             raw, timestamp, count);
     data[0] = raw;
@@ -2252,41 +2687,39 @@ int MPLSensor::inv_read_sensor_bias(int fd, long *data)
     count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
     if(count) {
         /* scale appropriately for MPL */
-        LOGV_IF(ENG_VERBOSE, 
-                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)", 
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
                 atol(x), atol(y), atol(z));
 
         data[0] = (long)(atol(x) / 10000 * (1L << 16));
         data[1] = (long)(atol(y) / 10000 * (1L << 16));
         data[2] = (long)(atol(z) / 10000 * (1L << 16));
 
-        LOGV_IF(ENG_VERBOSE, 
-                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)", 
+        LOGV_IF(ENG_VERBOSE,
+                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
                 data[0], data[1], data[2]);
     }
     return 0;
 }
 
-
 /** fill in the sensor list based on which sensors are configured.
  *  return the number of configured sensors.
  *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
  *  parameter len gives the length of the buffer pointed to by list
  */
-
 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
 {
     VFUNC_LOG;
 
     int numsensors;
 
-    if(len < (int)(7 * sizeof(sensor_t))) {
+    if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
         LOGE("HAL:sensor list too small, not populating.");
-        return 0;
+        return -(sizeof(sSensorList) / sizeof(sensor_t));
     }
 
     /* fill in the base values */
-    memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
+    memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
 
     /* first add gyro, accel and compass to the list */
 
@@ -2301,7 +2734,7 @@ int MPLSensor::populateSensorList(struct sensor_t *list, int len)
     mCompassSensor->fillList(&list[MagneticField]);
 
     if(1) {
-        numsensors = 7;
+        numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
         /* all sensors will be added to the list
            fill in orientation values */
         fillOrientation(list);
@@ -2335,10 +2768,13 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
             list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
             list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
             list[Accelerometer].power = ACCEL_MPU6050_POWER;
-
-            // TODO: for GED tablet
-            // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
-            list[Accelerometer].minDelay = 5000;
+            list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+            return;
+        } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+            list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+            list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+            list[Accelerometer].power = ACCEL_MPU6500_POWER;
+            list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
 
             return;
         } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
@@ -2347,7 +2783,7 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
             list[Accelerometer].power = ACCEL_MPU9150_POWER;
             list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
             return;
-        } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { 
+        } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
             list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
             list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
             list[Accelerometer].power = ACCEL_BMA250_POWER;
@@ -2378,10 +2814,12 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
         list[Gyro].maxRange = GYRO_MPU6050_RANGE;
         list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
         list[Gyro].power = GYRO_MPU6050_POWER;
-
-        // TODO: for GED tablet
-        // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
-        list[Gyro].minDelay = 5000;
+        list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+    } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
+        list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+        list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+        list[Gyro].power = GYRO_MPU6500_POWER;
+        list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
     } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
         list[Gyro].maxRange = GYRO_MPU9150_RANGE;
         list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
@@ -2395,6 +2833,12 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
         list[Gyro].power = GYRO_MPU3050_POWER;
         list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
     }
+
+    list[RawGyro].maxRange = list[Gyro].maxRange;
+    list[RawGyro].resolution = list[Gyro].resolution;
+    list[RawGyro].power = list[Gyro].power;
+    list[RawGyro].minDelay = list[Gyro].minDelay;
+
     return;
 }
 
@@ -2404,14 +2848,11 @@ void MPLSensor::fillRV(struct sensor_t *list)
     VFUNC_LOG;
 
     /* compute power on the fly */
-    list[RotationVector].power = list[Gyro].power + 
+    list[RotationVector].power = list[Gyro].power +
                                  list[Accelerometer].power +
                                  list[MagneticField].power;
     list[RotationVector].resolution = .00001;
     list[RotationVector].maxRange = 1.0;
-
-    // TODO: for GED tablet
-    // list[RotationVector].minDelay = 10000;
     list[RotationVector].minDelay = 5000;
 
     return;
@@ -2426,9 +2867,6 @@ void MPLSensor::fillOrientation(struct sensor_t *list)
                               list[MagneticField].power;
     list[Orientation].resolution = .00001;
     list[Orientation].maxRange = 360.0;
-
-    // TODO: for GED tablet
-    // list[Orientation].minDelay = 10000;
     list[Orientation].minDelay = 5000;
 
     return;
@@ -2443,9 +2881,6 @@ void MPLSensor::fillGravity( struct sensor_t *list)
                           list[MagneticField].power;
     list[Gravity].resolution = .00001;
     list[Gravity].maxRange = 9.81;
-
-    // TODO: for GED tablet
-    // list[Gravity].minDelay = 10000;
     list[Gravity].minDelay = 5000;
 
     return;
@@ -2460,9 +2895,6 @@ void MPLSensor::fillLinearAccel(struct sensor_t *list)
                           list[MagneticField].power;
     list[LinearAccel].resolution = list[Accelerometer].resolution;
     list[LinearAccel].maxRange = list[Accelerometer].maxRange;
-
-    // TODO: for GED tablet
-    // list[LinearAccel].minDelay = 10000;
     list[LinearAccel].minDelay = 5000;
 
     return;
@@ -2478,13 +2910,7 @@ int MPLSensor::inv_init_sysfs_attributes(void)
     char **dptr;
     int num;
 
-    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
-    // inv_get_sysfs_abs_path(sysfs_path);
-    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
-        ALOGE("MPLSensor failed get sysfs path");
-        return -1;
-    }
-    sysfs_names_ptr = 
+    sysfs_names_ptr =
             (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
     sptr = sysfs_names_ptr;
     if (sptr != NULL) {
@@ -2498,6 +2924,9 @@ int MPLSensor::inv_init_sysfs_attributes(void)
         return -1;
     }
 
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    // inv_get_sysfs_abs_path(sysfs_path);
+    inv_get_sysfs_path(sysfs_path);
     inv_get_iio_trigger_path(iio_trigger_path);
 
     sprintf(mpu.key, "%s%s", sysfs_path, "/key");
@@ -2512,6 +2941,7 @@ int MPLSensor::inv_init_sysfs_attributes(void)
     sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
     sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
     sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
     sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
     sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
 
@@ -2526,39 +2956,58 @@ int MPLSensor::inv_init_sysfs_attributes(void)
     sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
     sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
 
-#ifdef THIRD_PARTY_ACCEL    //BMA250
-    /* same as 'mpu.accel_enable' */
-    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/enable");
-    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate");
-    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA");
-    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA");
-    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/NA");   
-#else
     sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
     sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
-    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
-    sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
-    sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
-    sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
+    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
 
+#ifndef THIRD_PARTY_ACCEL //MPUxxxx
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
     // TODO: for bias settings
     sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
-
-    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
 #endif
 
+    sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
+    sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
+    sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
+
     sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
     sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
     sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
     sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
     sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
 
-#if 0
-    // test print sysfs paths
-    dptr = (char**)&mpu;
-    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
-        LOGE("HAL:sysfs path: %s", *dptr++);
-    }
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+
+    return 0;
+}
+
+bool MPLSensor::isMpu3050()
+{
+    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
+        return true;
+    else
+        return false;
+}
+
+int MPLSensor::isLowPowerQuatEnabled()
+{
+#ifdef ENABLE_LP_QUAT_FEAT
+    if (isMpu3050())
+        return 0;
+    return 1;
+#else
+    return 0;
 #endif
+}
+
+int MPLSensor::isDmpDisplayOrientationOn()
+{
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+    if (isMpu3050())
+        return 0;
+    return 1;
+#else
     return 0;
+#endif
 }
diff --git a/libsensors_iio/MPLSensor.h b/libsensors_iio/MPLSensor.h
index 3ef1ba3..3f6bb5b 100644
--- a/libsensors_iio/MPLSensor.h
+++ b/libsensors_iio/MPLSensor.h
@@ -1,288 +1,345 @@
-/*

-* Copyright (C) 2012 Invensense, Inc.

-*

-* Licensed under the Apache License, Version 2.0 (the "License");

-* you may not use this file except in compliance with the License.

-* You may obtain a copy of the License at

-*

-*      http://www.apache.org/licenses/LICENSE-2.0

-*

-* Unless required by applicable law or agreed to in writing, software

-* distributed under the License is distributed on an "AS IS" BASIS,

-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-* See the License for the specific language governing permissions and

-* limitations under the License.

-*/

-

-#ifndef ANDROID_MPL_SENSOR_H

-#define ANDROID_MPL_SENSOR_H

-

-#include <stdint.h>

-#include <errno.h>

-#include <sys/cdefs.h>

-#include <sys/types.h>

-#include <poll.h>

-#include <utils/Vector.h>

-#include <utils/KeyedVector.h>

-#include "sensors.h"

-#include "SensorBase.h"

-#include "InputEventReader.h"

-

-// TODO: change to wanted vendor

-#ifdef INVENSENSE_COMPASS_CAL

-

-#ifdef COMPASS_AMI306

-#warning "unified HAL for AMI306"

-#include "CompassSensor.IIO.AMI.h"

-#else

-#warning "unified HAL for MPU9150"

-#include "CompassSensor.IIO.9150.h"

-#endif

-

-#else

-#warning "unified HAL for AKM"

-#include "CompassSensor.AKM.h"

-#endif

-

-/*****************************************************************************/

-/* Sensors Enable/Disable Mask

- *****************************************************************************/

-#define MAX_CHIP_ID_LEN             (20)

-#define IIO_BUFFER_LENGTH           (100 * 2)

-

-#define INV_THREE_AXIS_GYRO         (0x000F)

-#define INV_THREE_AXIS_ACCEL        (0x0070)

-#define INV_THREE_AXIS_COMPASS      (0x0380)

-#define INV_ALL_SENSORS             (0x7FFF)

-

-#ifdef INVENSENSE_COMPASS_CAL

-#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \

-                                      | INV_THREE_AXIS_COMPASS \

-                                      | INV_THREE_AXIS_GYRO)

-#else

-// TODO: ID_M = 2 even for 3rd-party solution

-#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \

-                                      | INV_THREE_AXIS_COMPASS \

-                                      | INV_THREE_AXIS_GYRO)

-#endif

-

-// bit mask of current active features (mFeatureActiveMask)

-#define INV_COMPASS_CAL              0x01

-#define INV_COMPASS_FIT              0x02

-#define INV_DMP_QUATERNION           0x04

-

-/*****************************************************************************/

-/** MPLSensor implementation which fits into the HAL example for crespo provided

- *  by Google.

- *  WARNING: there may only be one instance of MPLSensor, ever.

- */

- 

-class MPLSensor: public SensorBase

-{

-    typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);

-

-public:

-    MPLSensor(CompassSensor *);

-    virtual ~MPLSensor();

-

-    enum

-    {

-        Gyro = 0,

-        Accelerometer,

-        MagneticField,

-        Orientation,

-        RotationVector,

-        LinearAccel,

-        Gravity,

-        numSensors

-    };

-

-    virtual int setDelay(int32_t handle, int64_t ns);

-    virtual int enable(int32_t handle, int enabled);

-    int32_t getEnableMask() { return mEnabled; }

-

-    virtual int readEvents(sensors_event_t *data, int count);

-    virtual int getFd() const;

-    virtual int getAccelFd() const;

-    virtual int getCompassFd() const;

-    virtual int getPollTime();

-    virtual bool hasPendingEvents() const;

-    virtual void sleepEvent();

-    virtual void wakeEvent();

-    int populateSensorList(struct sensor_t *list, int len);

-    void cbProcData();

-

-    // Do not work with this object unless it is initialized

-    bool isValid() { return mMplSensorInitialized; };

-

-    //static pointer to the object that will handle callbacks

-    static MPLSensor* gMPLSensor;

-

-    //AKM HAL Integration

-    //void set_compass(long ready, long x, long y, long z, long accuracy);

-    int executeOnData(sensors_event_t* data, int count);

-    int readAccelEvents(sensors_event_t* data, int count);

-    int readCompassEvents(sensors_event_t* data, int count);

-

-protected:

-    // Lets us know if the constructor was actually able to finish its job.

-    // E.g. false if init sysfs failed.

-    bool mMplSensorInitialized;

-    CompassSensor *mCompassSensor;

-

-    int gyroHandler(sensors_event_t *data);

-    int accelHandler(sensors_event_t *data);

-    int compassHandler(sensors_event_t *data);

-    int rvHandler(sensors_event_t *data);

-    int laHandler(sensors_event_t *data);

-    int gravHandler(sensors_event_t *data);

-    int orienHandler(sensors_event_t *data);

-    void calcOrientationSensor(float *Rx, float *Val);

-    virtual int update_delay();

-

-    void inv_set_device_properties();

-    int inv_constructor_init();

-    int inv_constructor_default_enable();

-    int setGyroInitialState();

-    int setAccelInitialState();

-    int masterEnable(int en);

-    int onPower(int en);

-    int enableLPQuaternion(int);

-    int enableQuaternionData(int);

-    int onDMP(int);

-    int enableGyro(int en);

-    int enableAccel(int en);

-    int enableCompass(int en);

-    void computeLocalSensorMask(int enabled_sensors);

-    int enableSensors(unsigned long sensors, int en, uint32_t changed);

-    int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);

-    int update_delay_sysfs_sensor(int fd, uint64_t ns);

-    int inv_float_to_q16(float *fdata, long *ldata);

-    int inv_long_to_q16(long *fdata, long *ldata);

-    int inv_float_to_round(float *fdata, long *ldata);

-    int inv_float_to_round2(float *fdata, short *sdata);

-    int inv_read_temperature(long long *data);

-    int inv_read_dmp_state(int fd);

-    int inv_read_sensor_bias(int fd, long *data);

-    void inv_get_sensors_orientation(void);

-    int inv_init_sysfs_attributes(void);

-    void setCompassDelay(int64_t ns);

-    void enable_iio_sysfs(void);

-    int enableTap(int);

-    int enableFlick(int);

-    int enablePedometer(int);

-    int checkLPQuaternion();

-

-    int mNewData;   // flag indicating that the MPL calculated new output values

-    int mDmpStarted;

-    long mMasterSensorMask;

-    long mLocalSensorMask;

-    int mPollTime;

-    bool mHaveGoodMpuCal;   // flag indicating that the cal file can be written

-    int mGyroAccuracy;      // value indicating the quality of the gyro calibr.

-    int mAccelAccuracy;     // value indicating the quality of the accel calibr.

-    struct pollfd mPollFds[5];

-    int mSampleCount;

-    pthread_mutex_t mMplMutex;

-    pthread_mutex_t mHALMutex;

-

-    char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];

-

-    int iio_fd;

-    int accel_fd;

-    int mpufifo_fd;

-    int gyro_temperature_fd;

-

-    uint32_t mEnabled;

-    uint32_t mOldEnabledMask;

-    sensors_event_t mPendingEvents[numSensors];

-    uint64_t mDelays[numSensors];

-    hfunc_t mHandlers[numSensors];

-    short mCachedGyroData[3];

-    long mCachedAccelData[3];

-    long mCachedCompassData[3];

-    long mCachedQuaternionData[4];

-    android::KeyedVector<int, int> mIrqFds;

-

-    InputEventCircularReader mAccelInputReader;

-    InputEventCircularReader mGyroInputReader;

-

-    bool mFirstRead;

-    short mTempScale;

-    short mTempOffset;

-    int64_t mTempCurrentTime;

-    int mAccelScale;

-

-    uint32_t mPendingMask;

-    unsigned long mSensorMask;

-

-    char chip_ID[MAX_CHIP_ID_LEN];

-

-    signed char mGyroOrientation[9];

-    signed char mAccelOrientation[9];

-

-    int64_t mSensorTimestamp;

-    int64_t mCompassTimestamp;

-

-    struct sysfs_attrbs {

-       char *chip_enable;

-       char *power_state;

-       char *dmp_firmware;

-       char *firmware_loaded;

-       char *dmp_on;

-       char *dmp_int_on;

-       char *tap_on;

-       char *key;

-       char *self_test;

-       char *temperature;

-       char *dmp_output_rate;

-

-       char *gyro_enable;

-       char *gyro_fifo_rate;

-       char *gyro_orient;

-       char *gyro_x_fifo_enable;

-       char *gyro_y_fifo_enable;

-       char *gyro_z_fifo_enable;

-

-       char *accel_enable;

-       char *accel_fifo_rate;

-       char *accel_fsr;

-       char *accel_bias;

-       char *accel_orient;

-       char *accel_x_fifo_enable;

-       char *accel_y_fifo_enable;

-       char *accel_z_fifo_enable;

-

-       char *quaternion_on;

-       char *in_quat_r_en;

-       char *in_quat_x_en;

-       char *in_quat_y_en;

-       char *in_quat_z_en;

-

-       char *in_timestamp_en;

-       char *trigger_name;

-       char *current_trigger;

-       char *buffer_length;

-    } mpu;

-

-    char *sysfs_names_ptr;

-    int mFeatureActiveMask;

-

-private:

-    /* added for dynamic get sensor list */

-    void fillAccel(const char* accel, struct sensor_t *list);

-    void fillGyro(const char* gyro, struct sensor_t *list);

-    void fillRV(struct sensor_t *list);

-    void fillOrientation(struct sensor_t *list);

-    void fillGravity(struct sensor_t *list);

-    void fillLinearAccel(struct sensor_t *list);

-    void storeCalibration();

-    void loadDMP();

-};

-

-extern "C" {

-    void setCallbackObject(MPLSensor*);

-    MPLSensor *getCallbackObject();

-}

-

-#endif  // ANDROID_MPL_SENSOR_H

+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SENSOR_H
+#define ANDROID_MPL_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+#ifdef INVENSENSE_COMPASS_CAL
+
+#ifdef COMPASS_YAS53x
+#warning "unified HAL for YAS53x"
+#include "CompassSensor.IIO.primary.h"
+#elif COMPASS_AMI306
+#warning "unified HAL for AMI306"
+#include "CompassSensor.IIO.primary.h"
+#else
+#warning "unified HAL for MPU9150"
+#include "CompassSensor.IIO.9150.h"
+#endif
+#else
+#warning "unified HAL for AKM"
+#include "CompassSensor.AKM.h"
+#endif
+
+/*****************************************************************************/
+/* Sensors Enable/Disable Mask
+ *****************************************************************************/
+#define MAX_CHIP_ID_LEN             (20)
+
+#define INV_THREE_AXIS_GYRO         (0x000F)
+#define INV_THREE_AXIS_ACCEL        (0x0070)
+#define INV_THREE_AXIS_COMPASS      (0x0380)
+#define INV_ALL_SENSORS             (0x7FFF)
+
+#ifdef INVENSENSE_COMPASS_CAL
+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \
+                                      | INV_THREE_AXIS_COMPASS \
+                                      | INV_THREE_AXIS_GYRO)
+#else
+#define ALL_MPL_SENSORS_NP          (INV_THREE_AXIS_ACCEL \
+                                      | INV_THREE_AXIS_COMPASS \
+                                      | INV_THREE_AXIS_GYRO)
+#endif
+
+// bit mask of current active features (mFeatureActiveMask)
+#define INV_COMPASS_CAL              0x01
+#define INV_COMPASS_FIT              0x02
+#define INV_DMP_QUATERNION           0x04
+#define INV_DMP_DISPL_ORIENTATION    0x08
+
+/* Uncomment to enable Low Power Quaternion */
+//#define ENABLE_LP_QUAT_FEAT
+
+/* Uncomment to enable DMP display orientation
+   (within the HAL, see below for Java framework) */
+//#define ENABLE_DMP_DISPL_ORIENT_FEAT
+
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION
+   sensor type (DMP screen orientation) to the Java framework.
+   NOTE:
+       need Invensense customized
+       'hardware/libhardware/include/hardware/sensors.h' to compile correctly.
+   NOTE:
+       need Invensense java framework changes to:
+       'frameworks/base/core/java/android/view/WindowOrientationListener.java'
+       'frameworks/base/core/java/android/hardware/Sensor.java'
+       'frameworks/base/core/java/android/hardware/SensorEvent.java'
+       for the 'Auto-rotate screen' to use this feature.
+*/
+#define ENABLE_DMP_SCREEN_AUTO_ROTATION
+#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly"
+#endif
+
+int isDmpScreenAutoRotationEnabled()
+{
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+    return 1;
+#else
+    return 0;
+#endif
+}
+
+int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;
+
+/*****************************************************************************/
+/** MPLSensor implementation which fits into the HAL example for crespo provided
+ *  by Google.
+ *  WARNING: there may only be one instance of MPLSensor, ever.
+ */
+
+class MPLSensor: public SensorBase
+{
+    typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
+
+public:
+
+    enum {
+        Gyro = 0,
+        RawGyro,
+        Accelerometer,
+        MagneticField,
+        Orientation,
+        RotationVector,
+        LinearAccel,
+        Gravity,
+        NUMSENSORS
+    };
+
+    MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
+    virtual ~MPLSensor();
+
+    virtual int setDelay(int32_t handle, int64_t ns);
+    virtual int enable(int32_t handle, int enabled);
+    int32_t getEnableMask() { return mEnabled; }
+
+    virtual int readEvents(sensors_event_t *data, int count);
+    virtual int getFd() const;
+    virtual int getAccelFd() const;
+    virtual int getCompassFd() const;
+    virtual int getPollTime();
+    virtual bool hasPendingEvents() const;
+    virtual void sleepEvent();
+    virtual void wakeEvent();
+    int populateSensorList(struct sensor_t *list, int len);
+    void cbProcData();
+
+    //static pointer to the object that will handle callbacks
+    static MPLSensor* gMPLSensor;
+
+    //AKM HAL Integration
+    //void set_compass(long ready, long x, long y, long z, long accuracy);
+    int executeOnData(sensors_event_t* data, int count);
+    int readAccelEvents(sensors_event_t* data, int count);
+    int readCompassEvents(sensors_event_t* data, int count);
+
+    int turnOffAccelFifo();
+    int enableDmpOrientation(int);
+    int dmpOrientHandler(int);
+    int readDmpOrientEvents(sensors_event_t* data, int count);
+    int getDmpOrientFd();
+    int openDmpOrientFd();
+    int closeDmpOrientFd();
+
+    int getDmpRate(int64_t *);
+    int checkDMPOrientation();
+
+protected:
+    CompassSensor *mCompassSensor;
+
+    int gyroHandler(sensors_event_t *data);
+    int rawGyroHandler(sensors_event_t *data);
+    int accelHandler(sensors_event_t *data);
+    int compassHandler(sensors_event_t *data);
+    int rvHandler(sensors_event_t *data);
+    int laHandler(sensors_event_t *data);
+    int gravHandler(sensors_event_t *data);
+    int orienHandler(sensors_event_t *data);
+    void calcOrientationSensor(float *Rx, float *Val);
+    virtual int update_delay();
+
+    void inv_set_device_properties();
+    int inv_constructor_init();
+    int inv_constructor_default_enable();
+    int setGyroInitialState();
+    int setAccelInitialState();
+    int masterEnable(int en);
+    int onPower(int en);
+    int enableLPQuaternion(int);
+    int enableQuaternionData(int);
+    int onDMP(int);
+    int enableGyro(int en);
+    int enableAccel(int en);
+    int enableCompass(int en);
+    void computeLocalSensorMask(int enabled_sensors);
+    int enableSensors(unsigned long sensors, int en, uint32_t changed);
+    int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
+    int inv_float_to_q16(float *fdata, long *ldata);
+    int inv_long_to_q16(long *fdata, long *ldata);
+    int inv_float_to_round(float *fdata, long *ldata);
+    int inv_float_to_round2(float *fdata, short *sdata);
+    int inv_read_temperature(long long *data);
+    int inv_read_dmp_state(int fd);
+    int inv_read_sensor_bias(int fd, long *data);
+    void inv_get_sensors_orientation(void);
+    int inv_init_sysfs_attributes(void);
+#ifdef COMPASS_YAS53x
+    int resetCompass(void);
+#endif
+    void setCompassDelay(int64_t ns);
+    void enable_iio_sysfs(void);
+    int enableTap(int);
+    int enableFlick(int);
+    int enablePedometer(int);
+    int checkLPQuaternion();
+
+    int mNewData;   // flag indicating that the MPL calculated new output values
+    int mDmpStarted;
+    long mMasterSensorMask;
+    long mLocalSensorMask;
+    int mPollTime;
+    bool mHaveGoodMpuCal;   // flag indicating that the cal file can be written
+    int mGyroAccuracy;      // value indicating the quality of the gyro calibr.
+    int mAccelAccuracy;     // value indicating the quality of the accel calibr.
+    int mCompassAccuracy;     // value indicating the quality of the compass calibr.
+    struct pollfd mPollFds[5];
+    int mSampleCount;
+    pthread_mutex_t mMplMutex;
+    pthread_mutex_t mHALMutex;
+
+    char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
+
+    int iio_fd;
+    int accel_fd;
+    int mpufifo_fd;
+    int gyro_temperature_fd;
+
+    int dmp_orient_fd;
+    int mDmpOrientationEnabled;
+
+    uint32_t mEnabled;
+    uint32_t mOldEnabledMask;
+    sensors_event_t mPendingEvents[NUMSENSORS];
+    int64_t mDelays[NUMSENSORS];
+    hfunc_t mHandlers[NUMSENSORS];
+    short mCachedGyroData[3];
+    long mCachedAccelData[3];
+    long mCachedCompassData[3];
+    long mCachedQuaternionData[4];
+    android::KeyedVector<int, int> mIrqFds;
+
+    InputEventCircularReader mAccelInputReader;
+    InputEventCircularReader mGyroInputReader;
+
+    bool mFirstRead;
+    short mTempScale;
+    short mTempOffset;
+    int64_t mTempCurrentTime;
+    int mAccelScale;
+
+    uint32_t mPendingMask;
+    unsigned long mSensorMask;
+
+    char chip_ID[MAX_CHIP_ID_LEN];
+
+    signed char mGyroOrientation[9];
+    signed char mAccelOrientation[9];
+
+    int64_t mSensorTimestamp;
+    int64_t mCompassTimestamp;
+
+    struct sysfs_attrbs {
+       char *chip_enable;
+       char *power_state;
+       char *dmp_firmware;
+       char *firmware_loaded;
+       char *dmp_on;
+       char *dmp_int_on;
+       char *dmp_event_int_on;
+       char *dmp_output_rate;
+       char *tap_on;
+       char *key;
+       char *self_test;
+       char *temperature;
+
+       char *gyro_enable;
+       char *gyro_fifo_rate;
+       char *gyro_orient;
+       char *gyro_x_fifo_enable;
+       char *gyro_y_fifo_enable;
+       char *gyro_z_fifo_enable;
+
+       char *accel_enable;
+       char *accel_fifo_rate;
+       char *accel_fsr;
+       char *accel_bias;
+       char *accel_orient;
+       char *accel_x_fifo_enable;
+       char *accel_y_fifo_enable;
+       char *accel_z_fifo_enable;
+
+       char *quaternion_on;
+       char *in_quat_r_en;
+       char *in_quat_x_en;
+       char *in_quat_y_en;
+       char *in_quat_z_en;
+
+       char *in_timestamp_en;
+       char *trigger_name;
+       char *current_trigger;
+       char *buffer_length;
+
+       char *display_orientation_on;
+       char *event_display_orientation;
+    } mpu;
+
+    char *sysfs_names_ptr;
+    int mFeatureActiveMask;
+
+private:
+    /* added for dynamic get sensor list */
+    void fillAccel(const char* accel, struct sensor_t *list);
+    void fillGyro(const char* gyro, struct sensor_t *list);
+    void fillRV(struct sensor_t *list);
+    void fillOrientation(struct sensor_t *list);
+    void fillGravity(struct sensor_t *list);
+    void fillLinearAccel(struct sensor_t *list);
+    void storeCalibration();
+    void loadDMP();
+    bool isMpu3050();
+    int isLowPowerQuatEnabled();
+    int isDmpDisplayOrientationOn();
+
+
+};
+
+extern "C" {
+    void setCallbackObject(MPLSensor*);
+    MPLSensor *getCallbackObject();
+}
+
+#endif  // ANDROID_MPL_SENSOR_H
diff --git a/libsensors_iio/MPLSupport.cpp b/libsensors_iio/MPLSupport.cpp
index a961d78..c0b7d2a 100644
--- a/libsensors_iio/MPLSupport.cpp
+++ b/libsensors_iio/MPLSupport.cpp
@@ -1,3 +1,19 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
 #include <MPLSupport.h>
 #include <string.h>
 #include <stdio.h>
@@ -5,6 +21,8 @@
 #include "SensorBase.h"
 #include <fcntl.h>
 
+#include "ml_sysfs_helper.h"
+
 int inv_read_data(char *fname, long *data)
 {
     VFUNC_LOG;
@@ -65,20 +83,13 @@ int enable_sysfs_sensor(int fd, int en)
     int err = 0;
 
     if (fd >= 0) {
-        char buf[2];
-        if (en) {
-            buf[0] = '1';
-            nb = write(fd, buf, 1);
-        } else {
-            buf[0] = '0';
-            nb = write(fd, buf, 1);
-        }
-        buf[1] = '\0';
+        char c = en ? '1' : '0';
+        nb = write(fd, &c, 1);
 
         if (nb <= 0) {
             err = errno;
             LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
-                 buf[0], nb, strerror(err), err);
+                 c, nb, strerror(err), err);
         }
         close(fd);
     } else {
@@ -111,6 +122,28 @@ int write_attribute_sensor(int fd, long data)
     return num_b;
 }
 
+/* This one DOES NOT close FDs for you */
+int write_attribute_sensor_continuous(int fd, long data)
+{
+    VFUNC_LOG;
+
+    int num_b = 0;
+
+    if (fd >= 0) {
+        char buf[80];
+        sprintf(buf, "%ld", data);
+        num_b = write(fd, buf, strlen(buf) + 1);
+        if (num_b <= 0) {
+            int err = errno;
+            LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
+        } else {
+            LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
+        }
+    }
+
+    return num_b;
+}
+
 int read_sysfs_int(char *filename, int *var)
 {
     int res=0;
@@ -119,10 +152,10 @@ int read_sysfs_int(char *filename, int *var)
     sysfsfp = fopen(filename, "r");
     if (sysfsfp!=NULL) {
         fscanf(sysfsfp, "%d\n", var);
-	fclose(sysfsfp);
+        fclose(sysfsfp);
     } else {
-        LOGE("HAL:ERR open file to read");
-        res= -1;   
+        res = errno;
+        LOGE("HAL:ERR open file %s to read with error %d", filename, res);
     }
     return res;
 }
@@ -135,10 +168,54 @@ int write_sysfs_int(char *filename, int var)
     sysfsfp = fopen(filename, "w");
     if (sysfsfp!=NULL) {
         fprintf(sysfsfp, "%d\n", var);
-	fclose(sysfsfp);
+        fclose(sysfsfp);
     } else {
-        LOGE("HAL:ERR open file to write");
-        res= -1;   
+        res = errno;
+        LOGE("HAL:ERR open file %s to read with error %d", filename, res);
     }
     return res;
 }
+
+int fill_dev_full_name_by_prefix(const char* dev_prefix,
+                                 char *dev_full_name, int len)
+{
+    char cand_name[20];
+    int prefix_len = strlen(dev_prefix);
+    strncpy(cand_name, dev_prefix, sizeof(cand_name) / sizeof(cand_name[0]));
+
+    // try adding a number, 0-9
+    for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]),
+                 "%d", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0;
+        }
+    }
+    // try adding a small letter, a-z
+    for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]),
+                 "%c", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0;
+        }
+    }
+    // try adding a capital letter, A-Z
+    for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]),
+                 "%c", cand_postfix);
+        int dev_num = find_type_by_name(cand_name, "iio:device");
+        if (dev_num != -ENODEV) {
+            strncpy(dev_full_name, cand_name, len);
+            return 0;
+        }
+    }
+    return 1;
+}
+
diff --git a/libsensors_iio/MPLSupport.h b/libsensors_iio/MPLSupport.h
index 73604ba..42d7643 100644
--- a/libsensors_iio/MPLSupport.h
+++ b/libsensors_iio/MPLSupport.h
@@ -19,11 +19,14 @@
 
 #include <stdint.h>
 
-    int inv_read_data(char *fname, long *data);
-    int read_attribute_sensor(int fd, char* data, unsigned int size);
-    int enable_sysfs_sensor(int fd, int en);
-    int write_attribute_sensor(int fd, long data);
-    int read_sysfs_int(char*, int*);
-    int write_sysfs_int(char*, int);
+int inv_read_data(char *fname, long *data);
+int read_attribute_sensor(int fd, char* data, unsigned int size);
+int enable_sysfs_sensor(int fd, int en);
+int write_attribute_sensor(int fd, long data);
+int write_attribute_sensor_continuous(int fd, long data);
+int read_sysfs_int(char*, int*);
+int write_sysfs_int(char*, int);
+int fill_dev_full_name_by_prefix(const char* dev_prefix,
+                                 char* dev_full_name, int len);
 
 #endif //  ANDROID_MPL_SUPPORT_H
diff --git a/libsensors_iio/SensorBase.cpp b/libsensors_iio/SensorBase.cpp
index fd0e2ca..0cf17af 100644
--- a/libsensors_iio/SensorBase.cpp
+++ b/libsensors_iio/SensorBase.cpp
@@ -1,143 +1,142 @@
-/*

-* Copyright (C) 2012 Invensense, Inc.

-*

-* Licensed under the Apache License, Version 2.0 (the "License");

-* you may not use this file except in compliance with the License.

-* You may obtain a copy of the License at

-*

-*      http://www.apache.org/licenses/LICENSE-2.0

-*

-* Unless required by applicable law or agreed to in writing, software

-* distributed under the License is distributed on an "AS IS" BASIS,

-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-* See the License for the specific language governing permissions and

-* limitations under the License.

-*/

-

-#include <fcntl.h>

-#include <errno.h>

-#include <math.h>

-#include <poll.h>

-#include <unistd.h>

-#include <dirent.h>

-#include <sys/select.h>

-#include <cutils/log.h>

-#include <linux/input.h>

-

-#include "SensorBase.h"

-#include "local_log_def.h"

-

-/*****************************************************************************/

-

-SensorBase::SensorBase(const char* dev_name,

-                       const char* data_name) : dev_name(dev_name),

-                                                data_name(data_name),

-                                                dev_fd(-1),

-                                                data_fd(-1)

-{

-    if (data_name) {

-        data_fd = openInput(data_name);

-    }

-}

-

-SensorBase::~SensorBase()

-{

-    if (data_fd >= 0) {

-        close(data_fd);

-    }

-    if (dev_fd >= 0) {

-        close(dev_fd);

-    }

-}

-

-int SensorBase::open_device()

-{

-    if (dev_fd<0 && dev_name) {

-        dev_fd = open(dev_name, O_RDONLY);

-        LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));

-    }

-    return 0;

-}

-

-int SensorBase::close_device()

-{

-    if (dev_fd >= 0) {

-        close(dev_fd);

-        dev_fd = -1;

-    }

-    return 0;

-}

-

-int SensorBase::getFd() const

-{

-    if (!data_name) {

-        return dev_fd;

-    }

-    return data_fd;

-}

-

-int SensorBase::setDelay(int32_t handle, int64_t ns)

-{

-    return 0;

-}

-

-bool SensorBase::hasPendingEvents() const

-{

-    return false;

-}

-

-int64_t SensorBase::getTimestamp()

-{

-    struct timespec t;

-    t.tv_sec = t.tv_nsec = 0;

-    clock_gettime(CLOCK_MONOTONIC, &t);

-    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;

-}

-

-int SensorBase::openInput(const char *inputName)

-{

-    int fd = -1;

-    const char *dirname = "/dev/input";

-    char devname[PATH_MAX];

-    char *filename;

-    DIR *dir;

-    struct dirent *de;

-    dir = opendir(dirname);

-    if(dir == NULL)

-        return -1;

-    strcpy(devname, dirname);

-    filename = devname + strlen(devname);

-    *filename++ = '/';

-    while((de = readdir(dir))) {

-        if(de->d_name[0] == '.' &&

-                (de->d_name[1] == '\0' ||

-                        (de->d_name[1] == '.' && de->d_name[2] == '\0')))

-            continue;

-        strcpy(filename, de->d_name);

-        fd = open(devname, O_RDONLY);

-        LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);

-        LOGI("path open %s", devname);

-        if (fd >= 0) {

-            char name[80];

-            if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {

-                name[0] = '\0';

-            }

-            LOGV_IF(EXTRA_VERBOSE, "name read %s", name);

-            if (!strcmp(name, inputName)) {

-                strcpy(input_name, filename);

-                break;

-            } else {

-                close(fd);

-                fd = -1;

-            }

-        }

-    }

-    closedir(dir);

-    LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);

-    return fd;

-}

-

-int SensorBase::enable(int32_t handle, int enabled)

-{

-    return 0;

-}

+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+
+#include "SensorBase.h"
+
+/*****************************************************************************/
+
+SensorBase::SensorBase(const char* dev_name,
+                       const char* data_name) : dev_name(dev_name),
+                                                data_name(data_name),
+                                                dev_fd(-1),
+                                                data_fd(-1)
+{
+    if (data_name) {
+        data_fd = openInput(data_name);
+    }
+}
+
+SensorBase::~SensorBase()
+{
+    if (data_fd >= 0) {
+        close(data_fd);
+    }
+    if (dev_fd >= 0) {
+        close(dev_fd);
+    }
+}
+
+int SensorBase::open_device()
+{
+    if (dev_fd<0 && dev_name) {
+        dev_fd = open(dev_name, O_RDONLY);
+        LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
+    }
+    return 0;
+}
+
+int SensorBase::close_device()
+{
+    if (dev_fd >= 0) {
+        close(dev_fd);
+        dev_fd = -1;
+    }
+    return 0;
+}
+
+int SensorBase::getFd() const
+{
+    if (!data_name) {
+        return dev_fd;
+    }
+    return data_fd;
+}
+
+int SensorBase::setDelay(int32_t handle, int64_t ns)
+{
+    return 0;
+}
+
+bool SensorBase::hasPendingEvents() const
+{
+    return false;
+}
+
+int64_t SensorBase::getTimestamp()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+}
+
+int SensorBase::openInput(const char *inputName)
+{
+    int fd = -1;
+    const char *dirname = "/dev/input";
+    char devname[PATH_MAX];
+    char *filename;
+    DIR *dir;
+    struct dirent *de;
+    dir = opendir(dirname);
+    if(dir == NULL)
+        return -1;
+    strcpy(devname, dirname);
+    filename = devname + strlen(devname);
+    *filename++ = '/';
+    while((de = readdir(dir))) {
+        if(de->d_name[0] == '.' &&
+                (de->d_name[1] == '\0' ||
+                        (de->d_name[1] == '.' && de->d_name[2] == '\0')))
+            continue;
+        strcpy(filename, de->d_name);
+        fd = open(devname, O_RDONLY);
+        LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
+        LOGI("path open %s", devname);
+        if (fd >= 0) {
+            char name[80];
+            if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
+                name[0] = '\0';
+            }
+            LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
+            if (!strcmp(name, inputName)) {
+                strcpy(input_name, filename);
+                break;
+            } else {
+                close(fd);
+                fd = -1;
+            }
+        }
+    }
+    closedir(dir);
+    LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
+    return fd;
+}
+
+int SensorBase::enable(int32_t handle, int enabled)
+{
+    return 0;
+}
diff --git a/libsensors_iio/SensorBase.h b/libsensors_iio/SensorBase.h
index d9abe92..ddd1a05 100644
--- a/libsensors_iio/SensorBase.h
+++ b/libsensors_iio/SensorBase.h
@@ -22,9 +22,42 @@
 #include <sys/cdefs.h>
 #include <sys/types.h>
 
+#if defined ANDROID_JELLYBEAN
+#warning "build for Jellybean"
+#define LOGV_IF ALOGV_IF
+#define LOGE_IF ALOGE_IF
+#define LOGI ALOGI
+#define LOGE ALOGE
+#define LOGV ALOGV
+#define LOGW ALOGW
+#else
+#warning "build for ICS or earlier version"
+#endif
+
+/* Log enablers, each of these independent */
+
+#define PROCESS_VERBOSE (0) /* process log messages */
+#define EXTRA_VERBOSE   (0) /* verbose log messages */
+#define SYSFS_VERBOSE   (0) /* log sysfs interactions as cat/echo for repro
+                               purpose on a shell */
+#define FUNC_ENTRY      (0) /* log entry in all one-time functions */
+
+/* Note that enabling this logs may affect performance */
+#define HANDLER_ENTRY   (0) /* log entry in all handler functions */
+#define ENG_VERBOSE     (0) /* log some a lot more info about the internals */
+#define INPUT_DATA      (0) /* log the data input from the events */
+#define HANDLER_DATA    (0) /* log the data fetched from the handlers */
+
+#define FUNC_LOG \
+            LOGV("%s", __PRETTY_FUNCTION__)
+#define VFUNC_LOG \
+            LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)
+#define VHANDLER_LOG \
+            LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
 #define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
 
 #define MAX_SYSFS_NAME_LEN  (100)
+#define IIO_BUFFER_LENGTH   (480)
 
 /*****************************************************************************/
 
diff --git a/libsensors_iio/libmllite.so b/libsensors_iio/libmllite.so
index ed13b13..c23e5b9 100644
--- a/libsensors_iio/libmllite.so
+++ b/libsensors_iio/libmllite.so
diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so
index e2ab461..23b4721 100644
--- a/libsensors_iio/libmplmpu.so
+++ b/libsensors_iio/libmplmpu.so
diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h
deleted file mode 100644
index b08fdf1..0000000
--- a/libsensors_iio/local_log_def.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef LOCAL_LOG_DEF_H

-#define LOCAL_LOG_DEF_H

-

-// -- workaround for different LOG definition in JellyBean --

-#define LOGV            ALOGV

-#define LOGV_IF         ALOGV_IF

-#define LOGD            ALOGD

-#define LOGD_IF         ALOGD_IF

-#define LOGI            ALOGI

-#define LOGI_IF         ALOGI_IF

-#define LOGW            ALOGW

-#define LOGW_IF         ALOGW_IF

-#define LOGE            ALOGE

-#define LOGE_IF         ALOGE_IF

-#define IF_LOGV         IF_ALOGV

-#define IF_LOGD         IF_ALOGD

-#define IF_LOGI         IF_ALOGI

-#define IF_LOGW         IF_ALOGW

-#define IF_LOGE         IF_ALOGE

-#define LOG_ASSERT      ALOG_ASSERT

-#define LOG                     ALOG

-#define IF_LOG          IF_ALOG

-// -- Workaround End --

-

-/* Log enablers, each of these independent */

-

-#define PROCESS_VERBOSE (0) /* process log messages */

-#define EXTRA_VERBOSE   (0) /* verbose log messages */

-#define SYSFS_VERBOSE   (0) /* log sysfs interactions as cat/echo for repro

-                               purpose on a shell */

-#define FUNC_ENTRY      (0) /* log entry in all one-time functions */

-

-/* Note that enabling this logs may affect performance */

-#define HANDLER_ENTRY   (0) /* log entry in all handler functions */

-#define ENG_VERBOSE     (0) /* log some a lot more info about the internals */

-#define INPUT_DATA      (0) /* log the data input from the events */

-#define HANDLER_DATA    (0) /* log the data fetched from the handlers */

-

-#define FUNC_LOG \

-            LOGV("%s", __PRETTY_FUNCTION__)

-#define VFUNC_LOG \

-            LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)

-#define VHANDLER_LOG \

-            LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)

-

-#endif

diff --git a/libsensors_iio/sensor_params.h b/libsensors_iio/sensor_params.h
index 88d5ba0..c5d6307 100644
--- a/libsensors_iio/sensor_params.h
+++ b/libsensors_iio/sensor_params.h
@@ -1,162 +1,194 @@
-/*

-* Copyright (C) 2012 Invensense, Inc.

-*

-* Licensed under the Apache License, Version 2.0 (the "License");

-* you may not use this file except in compliance with the License.

-* You may obtain a copy of the License at

-*

-*      http://www.apache.org/licenses/LICENSE-2.0

-*

-* Unless required by applicable law or agreed to in writing, software

-* distributed under the License is distributed on an "AS IS" BASIS,

-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-* See the License for the specific language governing permissions and

-* limitations under the License.

-*/

-

-#ifndef INV_SENSOR_PARAMS_H

-#define INV_SENSOR_PARAMS_H

-

-/* Physical parameters of the sensors supported by Invensense MPL */

-#define SENSORS_ROTATION_VECTOR_HANDLE  (ID_RV)

-#define SENSORS_LINEAR_ACCEL_HANDLE     (ID_LA)

-#define SENSORS_GRAVITY_HANDLE          (ID_GR)

-#define SENSORS_GYROSCOPE_HANDLE        (ID_GY)

-#define SENSORS_ACCELERATION_HANDLE     (ID_A)

-#define SENSORS_MAGNETIC_FIELD_HANDLE   (ID_M)

-#define SENSORS_ORIENTATION_HANDLE      (ID_O)

-

-/******************************************/

-//MPU9150 INV_COMPASS

-#define COMPASS_MPU9150_RANGE           (9830.f)

-#define COMPASS_MPU9150_RESOLUTION      (0.285f)

-#define COMPASS_MPU9150_POWER           (10.f)

-#define COMPASS_MPU9150_MINDELAY        (10000)

-//COMPASS_ID_AKM

-#define COMPASS_AKM8975_RANGE           (9830.f)

-#define COMPASS_AKM8975_RESOLUTION      (0.285f)

-#define COMPASS_AKM8975_POWER           (10.f)

-#define COMPASS_AKM8975_MINDELAY        (10000)

-//COMPASS_ID_AMI30X

-#define COMPASS_AMI30X_RANGE            (5461.f)

-#define COMPASS_AMI30X_RESOLUTION       (0.9f)

-#define COMPASS_AMI30X_POWER            (0.15f)

-//COMPASS_ID_AMI306

-#define COMPASS_AMI306_RANGE            (5461.f)

-#define COMPASS_AMI306_RESOLUTION       (0.9f)

-#define COMPASS_AMI306_POWER            (0.15f)

-#define COMPASS_AMI306_MINDELAY         (10000)

-//COMPASS_ID_YAS529

-#define COMPASS_YAS529_RANGE            (19660.f)

-#define COMPASS_YAS529_RESOLUTION       (0.012f)

-#define COMPASS_YAS529_POWER            (4.f)

-//COMPASS_ID_YAS530

-#define COMPASS_YAS530_RANGE            (8001.f)

-#define COMPASS_YAS530_RESOLUTION       (0.012f)

-#define COMPASS_YAS530_POWER            (4.f)

-#define COMPASS_YAS530_MINDELAY         (10000)

-//COMPASS_ID_HMC5883

-#define COMPASS_HMC5883_RANGE           (10673.f)

-#define COMPASS_HMC5883_RESOLUTION      (10.f)

-#define COMPASS_HMC5883_POWER           (0.24f)

-//COMPASS_ID_LSM303DLH

-#define COMPASS_LSM303DLH_RANGE         (10240.f)

-#define COMPASS_LSM303DLH_RESOLUTION    (1.f)

-#define COMPASS_LSM303DLH_POWER         (1.f)

-//COMPASS_ID_LSM303DLM

-#define COMPASS_LSM303DLM_RANGE         (10240.f)

-#define COMPASS_LSM303DLM_RESOLUTION    (1.f)

-#define COMPASS_LSM303DLM_POWER         (1.f)

-//COMPASS_ID_MMC314X

-#define COMPASS_MMC314X_RANGE           (400.f)

-#define COMPASS_MMC314X_RESOLUTION      (2.f)

-#define COMPASS_MMC314X_POWER           (0.55f)

-//COMPASS_ID_HSCDTD002B

-#define COMPASS_HSCDTD002B_RANGE        (9830.f)

-#define COMPASS_HSCDTD002B_RESOLUTION   (1.f)

-#define COMPASS_HSCDTD002B_POWER        (1.f)

-//COMPASS_ID_HSCDTD004A

-#define COMPASS_HSCDTD004A_RANGE        (9830.f)

-#define COMPASS_HSCDTD004A_RESOLUTION   (1.f)

-#define COMPASS_HSCDTD004A_POWER        (1.f)

-/*******************************************/

-//ACCEL_ID_MPU9150

-#define ACCEL_MPU9150_RANGE             (2.f * GRAVITY_EARTH)

-#define ACCEL_MPU9150_RESOLUTION        (0.004f * GRAVITY_EARTH)

-#define ACCEL_MPU9150_POWER             (0.f)

-#define ACCEL_MPU9150_MINDELAY          (1000)

-//ACCEL_ID_LIS331

-#define ACCEL_LIS331_RANGE              (2.48f * GRAVITY_EARTH)

-#define ACCEL_LIS331_RESOLUTION         (0.001f * GRAVITY_EARTH)

-#define ACCEL_LIS331_POWER              (1.f)

-//ACCEL_ID_LSM303DLX

-#define ACCEL_LSM303DLX_RANGE           (2.48f * GRAVITY_EARTH)

-#define ACCEL_LSM303DLX_RESOLUTION      (0.001f * GRAVITY_EARTH)

-#define ACCEL_LSM303DLX_POWER           (1.f)

-//ACCEL_ID_LIS3DH

-#define ACCEL_LIS3DH_RANGE              (2.48f * GRAVITY_EARTH)

-#define ACCEL_LIS3DH_RESOLUTION         (0.001f * GRAVITY_EARTH)

-#define ACCEL_LIS3DH_POWER              (1.f)

-//ACCEL_ID_KXSD9

-#define ACCEL_KXSD9_RANGE               (2.5006f * GRAVITY_EARTH)

-#define ACCEL_KXSD9_RESOLUTION          (0.001f * GRAVITY_EARTH)

-#define ACCEL_KXSD9_POWER               (1.f)

-//ACCEL_ID_KXTF9

-#define ACCEL_KXTF9_RANGE               (1.f * GRAVITY_EARTH)

-#define ACCEL_KXTF9_RESOLUTION          (0.033f * GRAVITY_EARTH)

-#define ACCEL_KXTF9_POWER               (0.35f)

-//ACCEL_ID_BMA150

-#define ACCEL_BMA150_RANGE              (2.f * GRAVITY_EARTH)

-#define ACCEL_BMA150_RESOLUTION         (0.004f * GRAVITY_EARTH)

-#define ACCEL_BMA150_POWER              (0.2f)

-//ACCEL_ID_BMA222

-#define ACCEL_BMA222_RANGE              (2.f * GRAVITY_EARTH)

-#define ACCEL_BMA222_RESOLUTION         (0.001f * GRAVITY_EARTH)

-#define ACCEL_BMA222_POWER              (0.1f)

-//ACCEL_ID_BMA250

-#define ACCEL_BMA250_RANGE              (2.f * GRAVITY_EARTH)

-#define ACCEL_BMA250_RESOLUTION         (0.00391f * GRAVITY_EARTH)

-#define ACCEL_BMA250_POWER              (0.139f)

-#define ACCEL_BMA250_MINDELAY           (1000)

-//ACCEL_ID_ADXL34X

-#define ACCEL_ADXL34X_RANGE             (2.f * GRAVITY_EARTH)

-#define ACCEL_ADXL34X_RESOLUTION        (0.001f * GRAVITY_EARTH)

-#define ACCEL_ADXL34X_POWER             (1.f)

-//ACCEL_ID_MMA8450

-#define ACCEL_MMA8450_RANGE             (2.f * GRAVITY_EARTH)

-#define ACCEL_MMA8450_RESOLUTION        (0.001f * GRAVITY_EARTH)

-#define ACCEL_MMA8450_POWER             (1.0f)

-//ACCEL_ID_MMA845X

-#define ACCEL_MMA845X_RANGE             (2.f * GRAVITY_EARTH)

-#define ACCEL_MMA845X_RESOLUTION        (0.001f * GRAVITY_EARTH)

-#define ACCEL_MMA845X_POWER             (1.f)

-//ACCEL_ID_MPU6050

-#define ACCEL_MPU6050_RANGE             (2.f * GRAVITY_EARTH)

-#define ACCEL_MPU6050_RESOLUTION        (0.004f * GRAVITY_EARTH)

-#define ACCEL_MPU6050_POWER             (0.f)

-#define ACCEL_MPU6050_MINDELAY          (1000)

-/******************************************/

-//GYRO MPU3050

-#define RAD_P_DEG                       (3.14159f / 180.f)

-#define GYRO_MPU3050_RANGE              (2000.f * RAD_P_DEG)

-#define GYRO_MPU3050_RESOLUTION         (32.8f * RAD_P_DEG)

-#define GYRO_MPU3050_POWER              (6.1f)

-#define GYRO_MPU3050_MINDELAY           (1000)

-//GYRO MPU6050

-#define GYRO_MPU6050_RANGE              (2000.f * RAD_P_DEG)

-#define GYRO_MPU6050_RESOLUTION         (16.4f * RAD_P_DEG)

-#define GYRO_MPU6050_POWER              (5.5f)

-#define GYRO_MPU6050_MINDELAY           (1000)

-//GYRO MPU9150

-#define GYRO_MPU9150_RANGE              (2000.f * RAD_P_DEG)

-#define GYRO_MPU9150_RESOLUTION         (16.4f * RAD_P_DEG)

-#define GYRO_MPU9150_POWER              (5.5f)

-#define GYRO_MPU9150_MINDELAY           (1000)

-//GYRO ITG3500

-#define GYRO_ITG3500_RANGE              (2000.f * RAD_P_DEG)

-#define GYRO_ITG3500_RESOLUTION         (16.4f * RAD_P_DEG)

-#define GYRO_ITG3500_POWER              (5.5f)

-#define GYRO_ITG3500_MINDELAY           (1000)

-

-#endif  /* INV_SENSOR_PARAMS_H */

-

+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef INV_SENSOR_PARAMS_H
+#define INV_SENSOR_PARAMS_H
+
+/* Physical parameters of the sensors supported by Invensense MPL */
+#define SENSORS_ROTATION_VECTOR_HANDLE    (ID_RV)
+#define SENSORS_LINEAR_ACCEL_HANDLE       (ID_LA)
+#define SENSORS_GRAVITY_HANDLE            (ID_GR)
+#define SENSORS_GYROSCOPE_HANDLE          (ID_GY)
+#define SENSORS_RAW_GYROSCOPE_HANDLE      (ID_RG)
+#define SENSORS_ACCELERATION_HANDLE       (ID_A)
+#define SENSORS_MAGNETIC_FIELD_HANDLE     (ID_M)
+#define SENSORS_ORIENTATION_HANDLE        (ID_O)
+#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO)
+
+/******************************************/
+//MPU9250 INV_COMPASS
+#define COMPASS_MPU9250_RANGE           (9830.f)
+#define COMPASS_MPU9250_RESOLUTION      (0.15f)
+#define COMPASS_MPU9250_POWER           (10.f)
+#define COMPASS_MPU9250_MINDELAY        (10000)
+//MPU9150 INV_COMPASS
+#define COMPASS_MPU9150_RANGE           (9830.f)
+#define COMPASS_MPU9150_RESOLUTION      (0.285f)
+#define COMPASS_MPU9150_POWER           (10.f)
+#define COMPASS_MPU9150_MINDELAY        (10000)
+//COMPASS_ID_AK8975
+#define COMPASS_AKM8975_RANGE           (9830.f)
+#define COMPASS_AKM8975_RESOLUTION      (0.285f)
+#define COMPASS_AKM8975_POWER           (10.f)
+#define COMPASS_AKM8975_MINDELAY        (10000)
+//COMPASS_ID_AK8963C
+#define COMPASS_AKM8963_RANGE           (9830.f)
+#define COMPASS_AKM8963_RESOLUTION      (0.15f)
+#define COMPASS_AKM8963_POWER           (10.f)
+#define COMPASS_AKM8963_MINDELAY        (10000)
+//COMPASS_ID_AMI30X
+#define COMPASS_AMI30X_RANGE            (5461.f)
+#define COMPASS_AMI30X_RESOLUTION       (0.9f)
+#define COMPASS_AMI30X_POWER            (0.15f)
+//COMPASS_ID_AMI306
+#define COMPASS_AMI306_RANGE            (5461.f)
+#define COMPASS_AMI306_RESOLUTION       (0.9f)
+#define COMPASS_AMI306_POWER            (0.15f)
+#define COMPASS_AMI306_MINDELAY         (10000)
+//COMPASS_ID_YAS529
+#define COMPASS_YAS529_RANGE            (19660.f)
+#define COMPASS_YAS529_RESOLUTION       (0.012f)
+#define COMPASS_YAS529_POWER            (4.f)
+//COMPASS_ID_YAS53x
+#define COMPASS_YAS53x_RANGE            (8001.f)
+#define COMPASS_YAS53x_RESOLUTION       (0.012f)
+#define COMPASS_YAS53x_POWER            (4.f)
+#define COMPASS_YAS53x_MINDELAY         (10000)
+//COMPASS_ID_HMC5883
+#define COMPASS_HMC5883_RANGE           (10673.f)
+#define COMPASS_HMC5883_RESOLUTION      (10.f)
+#define COMPASS_HMC5883_POWER           (0.24f)
+//COMPASS_ID_LSM303DLH
+#define COMPASS_LSM303DLH_RANGE         (10240.f)
+#define COMPASS_LSM303DLH_RESOLUTION    (1.f)
+#define COMPASS_LSM303DLH_POWER         (1.f)
+//COMPASS_ID_LSM303DLM
+#define COMPASS_LSM303DLM_RANGE         (10240.f)
+#define COMPASS_LSM303DLM_RESOLUTION    (1.f)
+#define COMPASS_LSM303DLM_POWER         (1.f)
+//COMPASS_ID_MMC314X
+#define COMPASS_MMC314X_RANGE           (400.f)
+#define COMPASS_MMC314X_RESOLUTION      (2.f)
+#define COMPASS_MMC314X_POWER           (0.55f)
+//COMPASS_ID_HSCDTD002B
+#define COMPASS_HSCDTD002B_RANGE        (9830.f)
+#define COMPASS_HSCDTD002B_RESOLUTION   (1.f)
+#define COMPASS_HSCDTD002B_POWER        (1.f)
+//COMPASS_ID_HSCDTD004A
+#define COMPASS_HSCDTD004A_RANGE        (9830.f)
+#define COMPASS_HSCDTD004A_RESOLUTION   (1.f)
+#define COMPASS_HSCDTD004A_POWER        (1.f)
+/*******************************************/
+//ACCEL_ID_MPU6500
+#define ACCEL_MPU6500_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6500_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6500_POWER             (0.f)
+#define ACCEL_MPU6500_MINDELAY          (1000)
+//ACCEL_ID_MPU9250
+#define ACCEL_MPU9250_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9250_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9250_POWER             (0.f)
+#define ACCEL_MPU9250_MINDELAY          (1000)
+//ACCEL_ID_MPU9150
+#define ACCEL_MPU9150_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_POWER             (0.f)
+#define ACCEL_MPU9150_MINDELAY          (1000)
+//ACCEL_ID_LIS331
+#define ACCEL_LIS331_RANGE              (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS331_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS331_POWER              (1.f)
+//ACCEL_ID_LSM303DLX
+#define ACCEL_LSM303DLX_RANGE           (2.48f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_RESOLUTION      (0.001f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_POWER           (1.f)
+//ACCEL_ID_LIS3DH
+#define ACCEL_LIS3DH_RANGE              (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_POWER              (1.f)
+//ACCEL_ID_KXSD9
+#define ACCEL_KXSD9_RANGE               (2.5006f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_RESOLUTION          (0.001f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_POWER               (1.f)
+//ACCEL_ID_KXTF9
+#define ACCEL_KXTF9_RANGE               (1.f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_RESOLUTION          (0.033f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_POWER               (0.35f)
+//ACCEL_ID_BMA150
+#define ACCEL_BMA150_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA150_RESOLUTION         (0.004f * GRAVITY_EARTH)
+#define ACCEL_BMA150_POWER              (0.2f)
+//ACCEL_ID_BMA222
+#define ACCEL_BMA222_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA222_RESOLUTION         (0.001f * GRAVITY_EARTH)
+#define ACCEL_BMA222_POWER              (0.1f)
+//ACCEL_ID_BMA250
+#define ACCEL_BMA250_RANGE              (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA250_RESOLUTION         (0.00391f * GRAVITY_EARTH)
+#define ACCEL_BMA250_POWER              (0.139f)
+#define ACCEL_BMA250_MINDELAY           (1000)
+//ACCEL_ID_ADXL34X
+#define ACCEL_ADXL34X_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_POWER             (1.f)
+//ACCEL_ID_MMA8450
+#define ACCEL_MMA8450_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_POWER             (1.0f)
+//ACCEL_ID_MMA845X
+#define ACCEL_MMA845X_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_RESOLUTION        (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_POWER             (1.f)
+//ACCEL_ID_MPU6050
+#define ACCEL_MPU6050_RANGE             (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_RESOLUTION        (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_POWER             (0.f)
+#define ACCEL_MPU6050_MINDELAY          (1000)
+/******************************************/
+//GYRO MPU3050
+#define RAD_P_DEG                       (3.14159f / 180.f)
+#define GYRO_MPU3050_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU3050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU3050_POWER              (6.1f)
+#define GYRO_MPU3050_MINDELAY           (1000)
+//GYRO MPU6050
+#define GYRO_MPU6050_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU6050_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU6050_POWER              (5.5f)
+#define GYRO_MPU6050_MINDELAY           (1000)
+//GYRO MPU9150
+#define GYRO_MPU9150_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9150_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9150_POWER              (5.5f)
+#define GYRO_MPU9150_MINDELAY           (1000)
+//GYRO MPU9250
+#define GYRO_MPU9250_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU9250_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU9250_POWER              (5.5f)
+#define GYRO_MPU9250_MINDELAY           (1000)
+//GYRO MPU6500
+#define GYRO_MPU6500_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_MPU6500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_MPU6500_POWER              (5.5f)
+#define GYRO_MPU6500_MINDELAY           (1000)
+//GYRO ITG3500
+#define GYRO_ITG3500_RANGE              (2000.f * RAD_P_DEG)
+#define GYRO_ITG3500_RESOLUTION         (2000.f / 32768.f * RAD_P_DEG)
+#define GYRO_ITG3500_POWER              (5.5f)
+#define GYRO_ITG3500_MINDELAY           (1000)
+
+#endif  /* INV_SENSOR_PARAMS_H */
+
diff --git a/libsensors_iio/sensors.h b/libsensors_iio/sensors.h
index 0556c10..d07e956 100644
--- a/libsensors_iio/sensors.h
+++ b/libsensors_iio/sensors.h
@@ -1,104 +1,97 @@
-/*

-* Copyright (C) 2012 Invensense, Inc.

-*

-* Licensed under the Apache License, Version 2.0 (the "License");

-* you may not use this file except in compliance with the License.

-* You may obtain a copy of the License at

-*

-*      http://www.apache.org/licenses/LICENSE-2.0

-*

-* Unless required by applicable law or agreed to in writing, software

-* distributed under the License is distributed on an "AS IS" BASIS,

-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-* See the License for the specific language governing permissions and

-* limitations under the License.

-*/

-

-#ifndef ANDROID_SENSORS_H

-#define ANDROID_SENSORS_H

-

-#include <stdint.h>

-#include <errno.h>

-#include <sys/cdefs.h>

-#include <sys/types.h>

-

-#include <linux/input.h>

-

-#include <hardware/hardware.h>

-#include <hardware/sensors.h>

-

-__BEGIN_DECLS

-

-/*****************************************************************************/

-

-#ifndef ARRAY_SIZE

-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))

-#endif

-

-#define ID_MPL_BASE (0)

-#define ID_GY (ID_MPL_BASE)

-#define ID_A  (ID_GY + 1)

-#define ID_M  (ID_A + 1)

-#define ID_O  (ID_M + 1)

-#define ID_RV (ID_O + 1)

-#define ID_LA (ID_RV + 1)

-#define ID_GR (ID_LA + 1)

-

-/*#define ID_MPL_BASE (0)

-#define ID_RV (ID_MPL_BASE)

-#define ID_LA (ID_RV + 1)

-#define ID_GR (ID_LA + 1)

-#define ID_GY (ID_GR + 1)

-#define ID_A  (ID_GY + 1)

-#define ID_M  (ID_A + 1)

-#define ID_O  (ID_M + 1)

-*/

-

-/*****************************************************************************/

-

-/*

- * The SENSORS Module

- */

-

-/* ITG3500 */

-#define EVENT_TYPE_GYRO_X          REL_X

-#define EVENT_TYPE_GYRO_Y          REL_Y

-#define EVENT_TYPE_GYRO_Z          REL_Z

-/* MPU6050 MPU9150 */

-#define EVENT_TYPE_IACCEL_X        REL_RX

-#define EVENT_TYPE_IACCEL_Y        REL_RY

-#define EVENT_TYPE_IACCEL_Z        REL_RZ

-/* MPU6050 MPU9150 */

-#define EVENT_TYPE_ICOMPASS_X      REL_X

-#define EVENT_TYPE_ICOMPASS_Y      REL_Y

-#define EVENT_TYPE_ICOMPASS_Z      REL_Z

-/* MPUxxxx */

-#define EVENT_TYPE_TIMESTAMP_HI    REL_MISC

-#define EVENT_TYPE_TIMESTAMP_LO    REL_WHEEL

-

-/* Accel BMA250 */

-#define EVENT_TYPE_ACCEL_X          ABS_X

-#define EVENT_TYPE_ACCEL_Y          ABS_Y

-#define EVENT_TYPE_ACCEL_Z          ABS_Z

-#define LSG                         (1000.0f)

-

-// conversion of acceleration data to SI units (m/s^2)

-#define RANGE_A                     (4*GRAVITY_EARTH)

-#define RESOLUTION_A                (GRAVITY_EARTH / LSG)

-#define CONVERT_A                   (GRAVITY_EARTH / LSG)

-#define CONVERT_A_X                 (CONVERT_A)

-#define CONVERT_A_Y                 (CONVERT_A)

-#define CONVERT_A_Z                 (CONVERT_A)

-

-/* Compass AKM8975 */

-#define EVENT_TYPE_MAGV_X           ABS_RX

-#define EVENT_TYPE_MAGV_Y           ABS_RY

-#define EVENT_TYPE_MAGV_Z           ABS_RZ

-#define EVENT_TYPE_MAGV_STATUS      ABS_RUDDER

-

-// conversion of magnetic data to uT units

-#define CONVERT_M                   (0.06f)

-

-__END_DECLS

-

-#endif  // ANDROID_SENSORS_H

+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_SENSORS_H
+#define ANDROID_SENSORS_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <hardware/hardware.h>
+#include <hardware/sensors.h>
+
+__BEGIN_DECLS
+
+/*****************************************************************************/
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+#endif
+
+enum {
+    ID_GY = 0,
+    ID_RG,
+    ID_A,
+    ID_M,
+    ID_O,
+    ID_RV,
+    ID_LA,
+    ID_GR,
+    ID_SO
+};
+
+/*****************************************************************************/
+
+/*
+ * The SENSORS Module
+ */
+
+/* ITG3500 */
+#define EVENT_TYPE_GYRO_X          REL_X
+#define EVENT_TYPE_GYRO_Y          REL_Y
+#define EVENT_TYPE_GYRO_Z          REL_Z
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_IACCEL_X        REL_RX
+#define EVENT_TYPE_IACCEL_Y        REL_RY
+#define EVENT_TYPE_IACCEL_Z        REL_RZ
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_ICOMPASS_X      REL_X
+#define EVENT_TYPE_ICOMPASS_Y      REL_Y
+#define EVENT_TYPE_ICOMPASS_Z      REL_Z
+/* MPUxxxx */
+#define EVENT_TYPE_TIMESTAMP_HI    REL_MISC
+#define EVENT_TYPE_TIMESTAMP_LO    REL_WHEEL
+
+/* Accel BMA250 */
+#define EVENT_TYPE_ACCEL_X          ABS_X
+#define EVENT_TYPE_ACCEL_Y          ABS_Y
+#define EVENT_TYPE_ACCEL_Z          ABS_Z
+#define LSG                         (1000.0f)
+
+// conversion of acceleration data to SI units (m/s^2)
+#define RANGE_A                     (4*GRAVITY_EARTH)
+#define RESOLUTION_A                (GRAVITY_EARTH / LSG)
+#define CONVERT_A                   (GRAVITY_EARTH / LSG)
+#define CONVERT_A_X                 (CONVERT_A)
+#define CONVERT_A_Y                 (CONVERT_A)
+#define CONVERT_A_Z                 (CONVERT_A)
+
+/* Compass AKM8975 */
+#define EVENT_TYPE_MAGV_X           ABS_RX
+#define EVENT_TYPE_MAGV_Y           ABS_RY
+#define EVENT_TYPE_MAGV_Z           ABS_RZ
+#define EVENT_TYPE_MAGV_STATUS      ABS_RUDDER
+
+// conversion of magnetic data to uT units
+#define CONVERT_M                   (0.06f)
+
+__END_DECLS
+
+#endif  // ANDROID_SENSORS_H
diff --git a/libsensors_iio/sensors_mpl.cpp b/libsensors_iio/sensors_mpl.cpp
index 990c5f5..50bf315 100644
--- a/libsensors_iio/sensors_mpl.cpp
+++ b/libsensors_iio/sensors_mpl.cpp
@@ -1,236 +1,260 @@
-/*

-* Copyright (C) 2012 Invensense, Inc.

-*

-* Licensed under the Apache License, Version 2.0 (the "License");

-* you may not use this file except in compliance with the License.

-* You may obtain a copy of the License at

-*

-*      http://www.apache.org/licenses/LICENSE-2.0

-*

-* Unless required by applicable law or agreed to in writing, software

-* distributed under the License is distributed on an "AS IS" BASIS,

-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-* See the License for the specific language governing permissions and

-* limitations under the License.

-*/

-

-#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)

-

-#include <hardware/sensors.h>

-#include <fcntl.h>

-#include <errno.h>

-#include <dirent.h>

-#include <math.h>

-#include <poll.h>

-#include <pthread.h>

-#include <stdlib.h>

-

-#include <linux/input.h>

-

-#include <utils/Atomic.h>

-#include <utils/Log.h>

-

-#include "sensors.h"

-#include "MPLSensor.h"

-#include "local_log_def.h"

-

-/*****************************************************************************/

-/* The SENSORS Module */

-#define LOCAL_SENSORS (7)

-

-static struct sensor_t sSensorList[7];

-static int numSensors = LOCAL_SENSORS;

-

-static int open_sensors(const struct hw_module_t* module, const char* id,

-                        struct hw_device_t** device);

-

-static int sensors__get_sensors_list(struct sensors_module_t* module,

-                                     struct sensor_t const** list)

-{

-    *list = sSensorList;

-    return numSensors;

-}

-

-static struct hw_module_methods_t sensors_module_methods = {

-        open: open_sensors

-};

-

-struct sensors_module_t HAL_MODULE_INFO_SYM = {

-        common: {

-                tag: HARDWARE_MODULE_TAG,

-                version_major: 1,

-                version_minor: 0,

-                id: SENSORS_HARDWARE_MODULE_ID,

-                name: "Invensense module",

-                author: "Invensense Inc.",

-                methods: &sensors_module_methods,

-        },

-        get_sensors_list: sensors__get_sensors_list,

-};

-

-struct sensors_poll_context_t {

-    struct sensors_poll_device_t device; // must be first

-

-    sensors_poll_context_t();

-    ~sensors_poll_context_t();

-    int activate(int handle, int enabled);

-    int setDelay(int handle, int64_t ns);

-    int pollEvents(sensors_event_t* data, int count);

-

-private:

-    enum {

-        mpl = 0,

-        compass,

-        numSensorDrivers,   // wake pipe goes here

-        numFds,

-    };

-

-    struct pollfd mPollFds[numSensorDrivers];

-    SensorBase *mSensor;

-    CompassSensor *mCompassSensor;

-};

-

-/******************************************************************************/

-

-sensors_poll_context_t::sensors_poll_context_t() {

-    VFUNC_LOG;

-

-    CompassSensor *mCompassSensor = new CompassSensor();

-    MPLSensor *mplSensor = new MPLSensor(mCompassSensor);

-

-    // setup the callback object for handing mpl callbacks

-    setCallbackObject(mplSensor);

-    

-    // populate the sensor list

-    numSensors =

-            mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));

-

-    mSensor = mplSensor;

-    mPollFds[mpl].fd = mSensor->getFd();

-    mPollFds[mpl].events = POLLIN;

-    mPollFds[mpl].revents = 0;

-

-    mPollFds[compass].fd = mCompassSensor->getFd();

-    mPollFds[compass].events = POLLIN;

-    mPollFds[compass].revents = 0;

-}

-

-sensors_poll_context_t::~sensors_poll_context_t() {

-    FUNC_LOG;

-    delete mSensor;

-    delete mCompassSensor;

-}

-

-int sensors_poll_context_t::activate(int handle, int enabled) {

-    FUNC_LOG;

-    return mSensor->enable(handle, enabled);

-}

-

-int sensors_poll_context_t::setDelay(int handle, int64_t ns)

-{

-    FUNC_LOG;

-    return mSensor->setDelay(handle, ns);

-}

-

-int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)

-{

-    VHANDLER_LOG;

-

-    int nbEvents = 0;

-    int nb, polltime = -1;

-

-    // look for new events

-    nb = poll(mPollFds, numSensorDrivers, polltime);

-

-    if (nb > 0) {

-        for (int i = 0; count && i < numSensorDrivers; i++) {

-            if (mPollFds[i].revents & POLLIN) {

-                nb = 0;

-                if (i == mpl) {

-                    nb = mSensor->readEvents(data, count);

-                }

-                else if (i == compass) {

-                    nb = ((MPLSensor*) mSensor)->readCompassEvents(data, count);

-                }

-/*

-                if (nb > 0) {

-                    count -= nb;

-                    nbEvents += nb;

-                    data += nb;

-                    mPollFds[i].revents = 0;

-                }

- */

-            }

-        }

-        nb = ((MPLSensor*) mSensor)->executeOnData(data, count);

-        if (nb > 0) {

-            count -= nb;

-            nbEvents += nb;

-            data += nb;

-            mPollFds[mpl].revents = 0;

-            mPollFds[compass].revents = 0;

-        }

-    }

-

-    return nbEvents;

-}

-

-/******************************************************************************/

-

-static int poll__close(struct hw_device_t *dev)

-{

-    FUNC_LOG;

-    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

-    if (ctx) {

-        delete ctx;

-    }

-    return 0;

-}

-

-static int poll__activate(struct sensors_poll_device_t *dev,

-                          int handle, int enabled)

-{

-    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

-    return ctx->activate(handle, enabled);

-}

-

-static int poll__setDelay(struct sensors_poll_device_t *dev,

-                          int handle, int64_t ns)

-{

-    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

-    int s= ctx->setDelay(handle, ns);

-    return s;

-}

-

-static int poll__poll(struct sensors_poll_device_t *dev,

-                      sensors_event_t* data, int count)

-{

-    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;

-    return ctx->pollEvents(data, count);

-}

-

-/******************************************************************************/

-

-/** Open a new instance of a sensor device using name */

-static int open_sensors(const struct hw_module_t* module, const char* id,

-                        struct hw_device_t** device)

-{

-    FUNC_LOG;

-    int status = -EINVAL;

-    sensors_poll_context_t *dev = new sensors_poll_context_t();

-

-    memset(&dev->device, 0, sizeof(sensors_poll_device_t));

-

-    dev->device.common.tag = HARDWARE_DEVICE_TAG;

-    dev->device.common.version  = 0;

-    dev->device.common.module   = const_cast<hw_module_t*>(module);

-    dev->device.common.close    = poll__close;

-    dev->device.activate        = poll__activate;

-    dev->device.setDelay        = poll__setDelay;

-    dev->device.poll            = poll__poll;

-

-    *device = &dev->device.common;

-    status = 0;

-

-    return status;

-}

+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*      http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
+
+#include <hardware/sensors.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <dirent.h>
+#include <math.h>
+#include <poll.h>
+#include <pthread.h>
+#include <stdlib.h>
+
+#include <linux/input.h>
+
+#include <utils/Atomic.h>
+#include <utils/Log.h>
+
+#include "sensors.h"
+#include "MPLSensor.h"
+
+/*****************************************************************************/
+/* The SENSORS Module */
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+#define LOCAL_SENSORS (MPLSensor::NUMSENSORS + 1)
+#else
+#define LOCAL_SENSORS MPLSensor::NUMSENSORS
+#endif
+
+/* Vendor-defined Accel Load Calibration File Method
+* @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame
+* @return '0' for a successful load, '1' otherwise
+* example: int AccelLoadConfig(long* offset);
+* End of Vendor-defined Accel Load Cal Method
+*/
+
+static struct sensor_t sSensorList[LOCAL_SENSORS];
+static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
+
+static int open_sensors(const struct hw_module_t* module, const char* id,
+                        struct hw_device_t** device);
+
+static int sensors__get_sensors_list(struct sensors_module_t* module,
+                                     struct sensor_t const** list)
+{
+    *list = sSensorList;
+    return sensors;
+}
+
+static struct hw_module_methods_t sensors_module_methods = {
+        open: open_sensors
+};
+
+struct sensors_module_t HAL_MODULE_INFO_SYM = {
+        common: {
+                tag: HARDWARE_MODULE_TAG,
+                version_major: 1,
+                version_minor: 0,
+                id: SENSORS_HARDWARE_MODULE_ID,
+                name: "Invensense module",
+                author: "Invensense Inc.",
+                methods: &sensors_module_methods,
+        },
+        get_sensors_list: sensors__get_sensors_list,
+};
+
+struct sensors_poll_context_t {
+    struct sensors_poll_device_t device; // must be first
+
+    sensors_poll_context_t();
+    ~sensors_poll_context_t();
+    int activate(int handle, int enabled);
+    int setDelay(int handle, int64_t ns);
+    int pollEvents(sensors_event_t* data, int count);
+
+private:
+    enum {
+        mpl = 0,
+        compass,
+        dmpOrient,
+        numSensorDrivers,   // wake pipe goes here
+        numFds,
+    };
+
+    struct pollfd mPollFds[numSensorDrivers];
+    SensorBase *mSensor;
+    CompassSensor *mCompassSensor;
+};
+
+/******************************************************************************/
+
+sensors_poll_context_t::sensors_poll_context_t() {
+    VFUNC_LOG;
+
+    mCompassSensor = new CompassSensor();
+    MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
+
+   /* For Vendor-defined Accel Calibration File Load
+    * Use the Following Constructor and Pass Your Load Cal File Function
+    *
+	* MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
+	*/
+
+    // setup the callback object for handing mpl callbacks
+    setCallbackObject(mplSensor);
+
+    // populate the sensor list
+    sensors =
+            mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
+
+    mSensor = mplSensor;
+    mPollFds[mpl].fd = mSensor->getFd();
+    mPollFds[mpl].events = POLLIN;
+    mPollFds[mpl].revents = 0;
+
+    mPollFds[compass].fd = mCompassSensor->getFd();
+    mPollFds[compass].events = POLLIN;
+    mPollFds[compass].revents = 0;
+
+    mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
+    mPollFds[dmpOrient].events = POLLPRI;
+    mPollFds[dmpOrient].revents = 0;
+}
+
+sensors_poll_context_t::~sensors_poll_context_t() {
+    FUNC_LOG;
+    delete mSensor;
+    delete mCompassSensor;
+}
+
+int sensors_poll_context_t::activate(int handle, int enabled) {
+    FUNC_LOG;
+    return mSensor->enable(handle, enabled);
+}
+
+int sensors_poll_context_t::setDelay(int handle, int64_t ns)
+{
+    FUNC_LOG;
+    return mSensor->setDelay(handle, ns);
+}
+
+int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
+{
+    VHANDLER_LOG;
+
+    int nbEvents = 0;
+    int nb, polltime = -1;
+
+    // look for new events
+    nb = poll(mPollFds, numSensorDrivers, polltime);
+
+    if (nb > 0) {
+        for (int i = 0; count && i < numSensorDrivers; i++) {
+            if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
+                nb = 0;
+                if (i == mpl) {
+                    nb = mSensor->readEvents(NULL, 0);
+                    mPollFds[i].revents = 0;
+                }
+                else if (i == compass) {
+                    nb = ((MPLSensor*) mSensor)->readCompassEvents(NULL, 0);
+                    mPollFds[i].revents = 0;
+                }
+            }
+        }
+        nb = ((MPLSensor*) mSensor)->executeOnData(data, count);
+        if (nb > 0) {
+            count -= nb;
+            nbEvents += nb;
+            data += nb;
+        }
+
+        if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {
+            nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);
+            mPollFds[dmpOrient].revents= 0;
+            if (isDmpScreenAutoRotationEnabled() && nb > 0) {
+                count -= nb;
+                nbEvents += nb;
+                data += nb;
+            }
+        }
+    }
+
+    return nbEvents;
+}
+
+/******************************************************************************/
+
+static int poll__close(struct hw_device_t *dev)
+{
+    FUNC_LOG;
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    if (ctx) {
+        delete ctx;
+    }
+    return 0;
+}
+
+static int poll__activate(struct sensors_poll_device_t *dev,
+                          int handle, int enabled)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->activate(handle, enabled);
+}
+
+static int poll__setDelay(struct sensors_poll_device_t *dev,
+                          int handle, int64_t ns)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    int s= ctx->setDelay(handle, ns);
+    return s;
+}
+
+static int poll__poll(struct sensors_poll_device_t *dev,
+                      sensors_event_t* data, int count)
+{
+    sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+    return ctx->pollEvents(data, count);
+}
+
+/******************************************************************************/
+
+/** Open a new instance of a sensor device using name */
+static int open_sensors(const struct hw_module_t* module, const char* id,
+                        struct hw_device_t** device)
+{
+    FUNC_LOG;
+    int status = -EINVAL;
+    sensors_poll_context_t *dev = new sensors_poll_context_t();
+
+    memset(&dev->device, 0, sizeof(sensors_poll_device_t));
+
+    dev->device.common.tag = HARDWARE_DEVICE_TAG;
+    dev->device.common.version  = 0;
+    dev->device.common.module   = const_cast<hw_module_t*>(module);
+    dev->device.common.close    = poll__close;
+    dev->device.activate        = poll__activate;
+    dev->device.setDelay        = poll__setDelay;
+    dev->device.poll            = poll__poll;
+
+    *device = &dev->device.common;
+    status = 0;
+
+    return status;
+}
diff --git a/libsensors_iio/software/build/android/common.mk b/libsensors_iio/software/build/android/common.mk
index b84a99c..84e7e9b 100644
--- a/libsensors_iio/software/build/android/common.mk
+++ b/libsensors_iio/software/build/android/common.mk
@@ -4,6 +4,9 @@ SHELL = /bin/bash
 ####################################################################################################
 ## defines
 
+# Build for Jellybean 
+BUILD_ANDROID_JELLYBEAN = 1
+
 ## libraries ##
 LIB_PREFIX = lib
 
@@ -16,23 +19,39 @@ TARGET ?= android
 
 MLLITE_LIB_NAME     ?= mllite
 MPL_LIB_NAME        ?= mplmpu
-HALWRAPPER_LIB_NAME ?= androidhal
 
 ## applications ##
 SHARED_APP_SUFFIX = -shared
 STATIC_APP_SUFFIX = -static
 
 ####################################################################################################
-## includes and linker
+## compile, includes, and linker
+
+ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
+endif
 
-ANDROID_LINK  = -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+ANDROID_LINK  = -nostdlib
+ANDROID_LINK += -fpic
+ANDROID_LINK += -Wl,--gc-sections 
+ANDROID_LINK += -Wl,--no-whole-archive 
 ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib 
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+ANDROID_LINK_EXECUTABLE  = $(ANDROID_LINK)
+ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker
+ifneq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+endif
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
 
 ANDROID_INCLUDES  = -I$(ANDROID_ROOT)/system/core/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include   # ICS
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
diff --git a/libsensors_iio/software/build/android/shared.mk b/libsensors_iio/software/build/android/shared.mk
index bc8548c..47dedfe 100644
--- a/libsensors_iio/software/build/android/shared.mk
+++ b/libsensors_iio/software/build/android/shared.mk
@@ -22,9 +22,9 @@ LIB_FOLDERS  = $(INV_ROOT)/core/mllite/build/$(TARGET)
 ifeq ($(BUILD_MPL),1)
 	LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET)
 endif
-APP_FOLDERS  = $(INV_ROOT)/simple_apps/console/linux/build/$(TARGET)
-APP_FOLDERS += $(INV_ROOT)/simple_apps/input_sub/build/$(TARGET)
+APP_FOLDERS  = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
 APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET)
 
 INSTALL_DIR = $(CURDIR)
 
diff --git a/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h
deleted file mode 100644
index fce28ae..0000000
--- a/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/**
- *  @brief    Provides helpful file IO wrappers for use with sysfs.
- *  @details  Based on Jonathan Cameron's @e iio_utils.h.
- */
-
-#ifndef _INV_SYSFS_UTILS_H_
-#define _INV_SYSFS_UTILS_H_
-
-/**
- *  struct inv_sysfs_names_s - Files needed by user applications.
- *  @buffer:		Ring buffer attached to FIFO.
- *  @enable:		Turns on HW-to-ring buffer flow.
- *  @raw_data:		Raw data from registers.
- *  @temperature:	Temperature data from register.
- *  @fifo_rate:		FIFO rate/ODR.
- *  @power_state:	Power state (this is a five-star comment).
- *  @fsr:		Full-scale range.
- *  @lpf:		Digital low pass filter.
- *  @scale:		LSBs / dps (or LSBs / Gs).
- *  @temp_scale:	LSBs / degrees C.
- *  @temp_offset:	Offset in LSBs.
- */
-struct inv_sysfs_names_s {
-
-	//Sysfs for ITG3500 & MPU6050
-	const char *buffer;
-	const char *enable;
-	const char *raw_data;		//Raw Gyro data
-	const char *temperature;
-	const char *fifo_rate;
-	const char *power_state;
-	const char *fsr;
-	const char *lpf;
-	const char *scale;			//Gyro scale
-	const char *temp_scale;
-	const char *temp_offset;
-	const char *self_test;
-	//Starting Sysfs available for MPU6050 only
-	const char *accel_en;
-	const char *accel_fifo_en;
-	const char *accel_fs;
-	const char *clock_source;
-	const char *early_suspend_en;
-	const char *firmware_loaded;
-	const char *gyro_en;
-	const char *gyro_fifo_en;
-	const char *key;
-	const char *raw_accel;
-	const char *reg_dump;
-	const char *tap_on;
-	const char *dmp_firmware;
-};
-
-/* File IO. Typically won't be called directly by user application, but they'll
- * be here for your enjoyment.
- */
-int inv_sysfs_write(char *filename, long data);
-int inv_sysfs_read(char *filename, long num_bytes, char *data);
-
-/* Helper APIs to extract specific data. */
-int inv_read_buffer(int fd, long *data, long long *timestamp);
-int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
-		 long long *timestamp);
-int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
-			     long long *timestamp);
-int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data);
-int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data);
-int inv_read_scale(const struct inv_sysfs_names_s *names, float *data);
-int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data);
-int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data);
-int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data);
-int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data);
-int inv_write_power_state(const struct inv_sysfs_names_s *names, char data);
-
-/* Scaled data. */
-int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
-		 long long *timestamp);
-int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
-                      long long *timestamp);
-
-#endif  /* #ifndef _INV_SYSFS_UTILS_H_ */
-
-
diff --git a/libsensors_iio/software/core/HAL/include/mlos.h b/libsensors_iio/software/core/HAL/include/mlos.h
deleted file mode 100644
index ce06b07..0000000
--- a/libsensors_iio/software/core/HAL/include/mlos.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-#ifndef _MLOS_H
-#define _MLOS_H
-
-#ifndef __KERNEL__
-#include <stdio.h>
-#endif
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#if defined(LINUX) || defined(__KERNEL__)
-typedef unsigned int HANDLE;
-#endif
-
-	/* ------------ */
-	/* - Defines. - */
-	/* ------------ */
-
-	/* - MLOSCreateFile defines. - */
-
-#define MLOS_GENERIC_READ         ((unsigned int)0x80000000)
-#define MLOS_GENERIC_WRITE        ((unsigned int)0x40000000)
-#define MLOS_FILE_SHARE_READ      ((unsigned int)0x00000001)
-#define MLOS_FILE_SHARE_WRITE     ((unsigned int)0x00000002)
-#define MLOS_OPEN_EXISTING        ((unsigned int)0x00000003)
-
-	/* ---------- */
-	/* - Enums. - */
-	/* ---------- */
-
-	/* --------------- */
-	/* - Structures. - */
-	/* --------------- */
-
-	/* --------------------- */
-	/* - Function p-types. - */
-	/* --------------------- */
-
-#ifndef __KERNEL__
-#include <string.h>
-	void *inv_malloc(unsigned int numBytes);
-	inv_error_t inv_free(void *ptr);
-	inv_error_t inv_create_mutex(HANDLE *mutex);
-	inv_error_t inv_lock_mutex(HANDLE mutex);
-	inv_error_t inv_unlock_mutex(HANDLE mutex);
-	FILE *inv_fopen(char *filename);
-	void inv_fclose(FILE *fp);
-
-    // Binary Semaphore
-    inv_error_t inv_create_binary_semaphore(HANDLE *semaphore);
-    inv_error_t inv_destroy_binary_semaphore(HANDLE handle);
-    inv_error_t inv_increment_binary_semaphore(HANDLE handle);
-    inv_error_t inv_decrement_binary_semaphore(HANDLE handle, long timeout_ms);
-
-	inv_error_t inv_destroy_mutex(HANDLE handle);
-
-	void inv_sleep(int mSecs);
-	inv_time_t inv_get_tick_count(void);
-
-	/* Kernel implmentations */
-#define GFP_KERNEL (0x70)
-	static inline void *kmalloc(size_t size,
-				    unsigned int gfp_flags)
-	{
-        (void)gfp_flags;
-		return inv_malloc((unsigned int)size);
-	}
-	static inline void *kzalloc(size_t size, unsigned int gfp_flags)
-	{
-		void *tmp = inv_malloc((unsigned int)size);
-        (void)gfp_flags;
-		if (tmp)
-			memset(tmp, 0, size);
-		return tmp;
-	}
-	static inline void kfree(void *ptr)
-	{
-		inv_free(ptr);
-	}
-	static inline void msleep(long msecs)
-	{
-		inv_sleep(msecs);
-	}
-	static inline void udelay(unsigned long usecs)
-	{
-		inv_sleep((usecs + 999) / 1000);
-	}
-#else
-#include <linux/delay.h>
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-#endif				/* _MLOS_H */
diff --git a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c
deleted file mode 100644
index c45badd..0000000
--- a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c
+++ /dev/null
@@ -1,317 +0,0 @@
-/**
- *  @brief    Provides helpful file IO wrappers for use with sysfs.
- *  @details  Based on Jonathan Cameron's @e iio_utils.h.
- */
-
-#include <string.h>
-#include <stdlib.h>
-#include <ctype.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <dirent.h>
-#include <errno.h>
-#include <unistd.h>
-#include "inv_sysfs_utils.h"
-
-/* General TODO list:
- * Select more reasonable string lengths or use fseek and malloc.
- */
-
-/**
- *  inv_sysfs_write() - Write an integer to a file.
- *  @filename:	Path to file.
- *  @data:	Value to write to file.
- *  Returns number of bytes written or a negative error code.
- */
-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;
-}
-
-/**
- *  inv_sysfs_read() - Read a string from a file.
- *  @filename:	Path to file.
- *  @num_bytes:	Number of bytes to read.
- *  @data:	Data from file.
- *  Returns number of bytes written or a negative error code.
- */
-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;
-}
-
-/**
- *  inv_read_buffer() - Read data from ring buffer.
- *  @fd:	File descriptor for buffer file.
- *  @data:	Data in hardware units.
- *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_buffer(int fd, long *data, long long *timestamp)
-{
-	char str[35];
-	int count;
-
-	count = read(fd, str, sizeof(str));
-	if (!count)
-		return count;
-	if (!timestamp)
-		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
-	else
-		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
-			&data[2], timestamp);
-
-	if (count < (timestamp?4:3))
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_raw() - Read raw data.
- *  @names:	Names of sysfs files.
- *  @data:	Data in hardware units.
- *  @timestamp:	Time when data was read from device. Use NULL if unsupported.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, 
-	long long *timestamp)
-{
-	char str[40];
-	int count;
-
-	count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str);
-	if (count < 0)
-		return count;
-	if (!timestamp)
-		count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
-	else
-		count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
-			&data[2], timestamp);
-	if (count < (timestamp?4:3))
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_temperature_raw() - Read temperature.
- *  @names:	Names of sysfs files.
- *  @data:	Data in hardware units.
- *  @timestamp:	Time when data was read from device.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
-	long long *timestamp)
-{
-	char str[25];
-	int count;
-
-	count = inv_sysfs_read((char*)names->temperature, sizeof(str), str);
-	if (count < 0)
-		return count;
-	count = sscanf(str, "%hd%lld", &data[0], timestamp);
-	if (count < 2)
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_fifo_rate() - Read fifo rate.
- *  @names:	Names of sysfs files.
- *  @data:	Fifo rate.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data)
-{
-	char str[8];
-	int count;
-
-	count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str);
-	if (count < 0)
-		return count;
-	count = sscanf(str, "%hd", data);
-	if (count < 1)
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_power_state() - Read power state.
- *  @names:	Names of sysfs files.
- *  @data:	1 if device is on.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data)
-{
-	char str[2];
-	int count;
-
-	count = inv_sysfs_read((char*)names->power_state, sizeof(str), str);
-	if (count < 0)
-		return count;
-	count = sscanf(str, "%hd", (short*)data);
-	if (count < 1)
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_scale() - Read scale.
- *  @names:	Names of sysfs files.
- *  @data:	1 if device is on.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_scale(const struct inv_sysfs_names_s *names, float *data)
-{
-	char str[5];
-	int count;
-
-	count = inv_sysfs_read((char*)names->scale, sizeof(str), str);
-	if (count < 0)
-		return count;
-	count = sscanf(str, "%f", data);
-	if (count < 1)
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_temp_scale() - Read temperature scale.
- *  @names:	Names of sysfs files.
- *  @data:	1 if device is on.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data)
-{
-	char str[4];
-	int count;
-
-	count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str);
-	if (count < 0)
-		return count;
-	count = sscanf(str, "%hd", data);
-	if (count < 1)
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_temp_offset() - Read temperature offset.
- *  @names:	Names of sysfs files.
- *  @data:	1 if device is on.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data)
-{
-	char str[4];
-	int count;
-
-	count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str);
-	if (count < 0)
-		return count;
-	count = sscanf(str, "%hd", data);
-	if (count < 1)
-		return -EAGAIN;
-	return count;
-}
-
-/**
- *  inv_read_q16() - Get data as q16 fixed point.
- *  @names:	Names of sysfs files.
- *  @data:	1 if device is on.
- *  @timestamp:	Time when data was read from device.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
-	long long *timestamp)
-{
-	int count;
-	short raw[3];
-	float scale;
-	count = inv_read_raw(names, (long*)raw, timestamp);
-	count += inv_read_scale(names, &scale);
-	data[0] = (long)(raw[0] * (65536.f / scale));
-	data[1] = (long)(raw[1] * (65536.f / scale));
-	data[2] = (long)(raw[2] * (65536.f / scale));
-	return count;
-}
-
-/**
- *  inv_read_q16() - Get temperature data as q16 fixed point.
- *  @names:	Names of sysfs files.
- *  @data:	1 if device is on.
- *  @timestamp:	Time when data was read from device.
- *  Returns number of bytes read or a negative error code.
- */
-int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
-	long long *timestamp)
-{
-	int count = 0;
-	short raw;
-	static short scale, offset;
-	static unsigned char first_read = 1;
-
-	if (first_read) {
-		count += inv_read_temp_scale(names, &scale);
-		count += inv_read_temp_offset(names, &offset);
-		first_read = 0;
-	}
-	count += inv_read_temperature_raw(names, &raw, timestamp);
-	data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f);
-
-	return count;
-}
-
-/**
- *  inv_write_fifo_rate() - Write fifo rate.
- *  @names:	Names of sysfs files.
- *  @data:	Fifo rate.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data)
-{
-	return inv_sysfs_write((char*)names->fifo_rate, (long)data);
-}
-
-/**
- *  inv_write_buffer_enable() - Enable/disable buffer in /dev.
- *  @names:	Names of sysfs files.
- *  @data:	Fifo rate.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data)
-{
-	return inv_sysfs_write((char*)names->enable, (long)data);
-}
-
-/**
- *  inv_write_power_state() - Turn device on/off.
- *  @names:	Names of sysfs files.
- *  @data:	1 to turn on.
- *  Returns number of bytes written or a negative error code.
- */
-int inv_write_power_state(const struct inv_sysfs_names_s *names, char data)
-{
-	return inv_sysfs_write((char*)names->power_state, (long)data);
-}
-
-
diff --git a/libsensors_iio/software/core/HAL/linux/mlos_linux.c b/libsensors_iio/software/core/HAL/linux/mlos_linux.c
deleted file mode 100644
index 75f062e..0000000
--- a/libsensors_iio/software/core/HAL/linux/mlos_linux.c
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-/*******************************************************************************
- *
- * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- *  @defgroup MLOS
- *  @brief OS Interface.
- *
- *  @{
- *      @file mlos.c
- *      @brief OS Interface.
-**/
-
-/* ------------- */
-/* - Includes. - */
-/* ------------- */
-
-#include <sys/time.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdlib.h>
-
-#include "stdint_invensense.h"
-
-#include "mlos.h"
-#include <errno.h>
-
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/**
- *  @brief  Allocate space
- *  @param  numBytes  number of bytes
- *  @return pointer to allocated space
-**/
-void *inv_malloc(unsigned int numBytes)
-{
-    // Allocate space.
-    void *allocPtr = malloc(numBytes);
-    return allocPtr;
-}
-
-
-/**
- *  @brief  Free allocated space
- *  @param  ptr pointer to space to deallocate
- *  @return error code.
-**/
-inv_error_t inv_free(void *ptr)
-{
-    // Deallocate space.
-    free(ptr);
-
-    return INV_SUCCESS;
-}
-
-
-/**
- *  @brief  Mutex create function
- *  @param  mutex   pointer to mutex handle
- *  @return error code.
- */
-inv_error_t inv_create_mutex(HANDLE *mutex)
-{
-    int res;
-    pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t));
-    if(pm == NULL) 
-        return INV_ERROR;
-
-    res = pthread_mutex_init(pm, NULL);
-    if(res == -1) {
-        free(pm);
-        return INV_ERROR_OS_CREATE_FAILED;
-    }
-
-    *mutex = (HANDLE)pm;
-
-    return INV_SUCCESS;
-}
-
-
-/**
- *  @brief  Mutex lock function
- *  @param  mutex   Mutex handle
- *  @return error code.
- */
-inv_error_t inv_lock_mutex(HANDLE mutex)
-{
-    int res;
-    pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
-
-    res = pthread_mutex_lock(pm);
-    if(res == -1) 
-        return INV_ERROR_OS_LOCK_FAILED;
-
-    return INV_SUCCESS;
-}
-
-
-/**
- *  @brief  Mutex unlock function
- *  @param  mutex   mutex handle
- *  @return error code.
-**/
-inv_error_t inv_unlock_mutex(HANDLE mutex)
-{
-    int res;
-    pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
-
-    res = pthread_mutex_unlock(pm);
-    if(res == -1) 
-        return INV_ERROR_OS_LOCK_FAILED;
-
-    return INV_SUCCESS;
-}
-
-
-/**
- *  @brief  open file
- *  @param  filename    name of the file to open.
- *  @return error code.
- */
-FILE *inv_fopen(char *filename)
-{
-    FILE *fp = fopen(filename,"r");
-    return fp;
-}
-
-
-/**
- *  @brief  close the file.
- *  @param  fp  handle to file to close.
- *  @return error code.
- */
-void inv_fclose(FILE *fp)
-{
-    fclose(fp);
-}
-
-/**
- *  @brief  Close Handle
- *  @param  handle  handle to the resource.
- *  @return Zero if success, an error code otherwise.
- */
-inv_error_t inv_destroy_mutex(HANDLE handle)
-{
-    int error;
-    pthread_mutex_t *pm = (pthread_mutex_t*)handle;
-    error = pthread_mutex_destroy(pm);
-    if (error) {
-        return errno;
-    }
-    free((void*) handle);
-    
-    return INV_SUCCESS;}
-
-
-/**
- *  @brief  Sleep function.
- */
-void inv_sleep(int mSecs)
-{
-    usleep(mSecs*1000);
-}
-
-
-/**
- *  @brief  get system's internal tick count.
- *          Used for time reference.
- *  @return current tick count.
- */
-unsigned long inv_get_tick_count()
-{
-    struct timeval tv;
-
-    if (gettimeofday(&tv, NULL) !=0)
-        return 0;
-
-    return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
-}
-
-  /**********************/
- /** @} */ /* defgroup */
-/**********************/
-
diff --git a/libsensors_iio/software/core/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h
index 9b29695..4391226 100644
--- a/libsensors_iio/software/core/driver/include/linux/mpu.h
+++ b/libsensors_iio/software/core/driver/include/linux/mpu.h
@@ -27,135 +27,8 @@
 #ifdef __KERNEL__
 #include <linux/types.h>
 #include <linux/ioctl.h>
-#elif defined LINUX
-#include <sys/ioctl.h>
-#include <linux/types.h>
-#else
-#include "mltypes.h"
 #endif
 
-struct mpu_read_write {
-	/* Memory address or register address depending on ioctl */
-	__u16 address;
-	__u16 length;
-	__u8 *data;
-};
-
-enum mpuirq_data_type {
-	MPUIRQ_DATA_TYPE_MPU_DATA_READY_IRQ,
-	MPUIRQ_DATA_TYPE_MPU_FIFO_READY_IRQ,
-	MPUIRQ_DATA_TYPE_SLAVE_IRQ,
-	MPUIRQ_DATA_TYPE_PM_EVENT,
-	MPUIRQ_DATA_TYPE_NUM_TYPES,
-};
-
-/* User space PM event notification */
-#define MPU_PM_EVENT_SUSPEND_PREPARE (3)
-#define MPU_PM_EVENT_POST_SUSPEND    (4)
-
-/**
- * struct mpuirq_data - structure to report what and when
- * @interruptcount	: The number of times this IRQ has occured since open
- * @irqtime		: monotonic time of the IRQ in ns
- * @data_type		: The type of this IRQ enum mpuirq_data_type
- * @data		: Data associated with this IRQ
- */
-struct mpuirq_data {
-	__u32 interruptcount;
-	__s64 irqtime_ns;
-	__u32 data_type;
-	__s32 data;
-};
-
-enum ext_slave_config_key {
-	/* TODO: Remove these first six. */
-	MPU_SLAVE_CONFIG_ODR_SUSPEND,
-	MPU_SLAVE_CONFIG_ODR_RESUME,
-	MPU_SLAVE_CONFIG_FSR_SUSPEND,
-	MPU_SLAVE_CONFIG_FSR_RESUME,
-	MPU_SLAVE_CONFIG_IRQ_SUSPEND,
-	MPU_SLAVE_CONFIG_IRQ_RESUME,
-	MPU_SLAVE_CONFIG_ODR,
-	MPU_SLAVE_CONFIG_FSR,
-	MPU_SLAVE_CONFIG_MOT_THS,
-	MPU_SLAVE_CONFIG_NMOT_THS,
-	MPU_SLAVE_CONFIG_MOT_DUR,
-	MPU_SLAVE_CONFIG_NMOT_DUR,
-	MPU_SLAVE_CONFIG_IRQ,
-	MPU_SLAVE_WRITE_REGISTERS,
-	MPU_SLAVE_READ_REGISTERS,
-	MPU_SLAVE_CONFIG_INTERNAL_REFERENCE,
-	/* AMI 306 specific config keys */
-	MPU_SLAVE_PARAM,
-	MPU_SLAVE_WINDOW,
-	MPU_SLAVE_READWINPARAMS,
-	MPU_SLAVE_SEARCHOFFSET,
-	/* MPU3050 and MPU6050 Keys */
-	MPU_SLAVE_INT_CONFIG,
-	MPU_SLAVE_EXT_SYNC,
-	MPU_SLAVE_FULL_SCALE,
-	MPU_SLAVE_LPF,
-	MPU_SLAVE_CLK_SRC,
-	MPU_SLAVE_DIVIDER,
-	MPU_SLAVE_DMP_ENABLE,
-	MPU_SLAVE_FIFO_ENABLE,
-	MPU_SLAVE_DMP_CFG1,
-	MPU_SLAVE_DMP_CFG2,
-	MPU_SLAVE_TC,
-	MPU_SLAVE_GYRO,
-	MPU_SLAVE_ADDR,
-	MPU_SLAVE_PRODUCT_REVISION,
-	MPU_SLAVE_SILICON_REVISION,
-	MPU_SLAVE_PRODUCT_ID,
-	MPU_SLAVE_GYRO_SENS_TRIM,
-	MPU_SLAVE_ACCEL_SENS_TRIM,
-	MPU_SLAVE_RAM,
-	/* -------------------------- */
-	MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS
-};
-
-/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */
-enum ext_slave_config_irq_type {
-	MPU_SLAVE_IRQ_TYPE_NONE,
-	MPU_SLAVE_IRQ_TYPE_MOTION,
-	MPU_SLAVE_IRQ_TYPE_DATA_READY,
-};
-
-/* Structure for the following IOCTS's
- * MPU_CONFIG_GYRO
- * MPU_CONFIG_ACCEL
- * MPU_CONFIG_COMPASS
- * MPU_CONFIG_PRESSURE
- * MPU_GET_CONFIG_GYRO
- * MPU_GET_CONFIG_ACCEL
- * MPU_GET_CONFIG_COMPASS
- * MPU_GET_CONFIG_PRESSURE
- *
- * @key one of enum ext_slave_config_key
- * @len length of data pointed to by data
- * @apply zero if communication with the chip is not necessary, false otherwise
- *        This flag can be used to select cached data or to refresh cashed data
- *        cache data to be pushed later or push immediately.  If true and the
- *        slave is on the secondary bus the MPU will first enger bypass mode
- *        before calling the slaves .config or .get_config funcion
- * @data pointer to the data to confgure or get
- */
-struct ext_slave_config {
-	__u8 key;
-	__u16 len;
-	__u8 apply;
-	void *data;
-};
-
-enum ext_slave_type {
-	EXT_SLAVE_TYPE_GYROSCOPE,
-	EXT_SLAVE_TYPE_ACCEL,
-	EXT_SLAVE_TYPE_COMPASS,
-	EXT_SLAVE_TYPE_PRESSURE,
-	/*EXT_SLAVE_TYPE_TEMPERATURE */
-
-	EXT_SLAVE_NUM_TYPES
-};
 enum secondary_slave_type {
 	SECONDARY_SLAVE_TYPE_NONE,
 	SECONDARY_SLAVE_TYPE_ACCEL,
@@ -204,127 +77,6 @@ enum ext_slave_id {
 };
 
 #define INV_PROD_KEY(ver, rev) (ver * 100 + rev)
-
-enum ext_slave_endian {
-	EXT_SLAVE_BIG_ENDIAN,
-	EXT_SLAVE_LITTLE_ENDIAN,
-	EXT_SLAVE_FS8_BIG_ENDIAN,
-	EXT_SLAVE_FS16_BIG_ENDIAN,
-};
-
-enum ext_slave_bus {
-	EXT_SLAVE_BUS_INVALID = -1,
-	EXT_SLAVE_BUS_PRIMARY = 0,
-	EXT_SLAVE_BUS_SECONDARY = 1
-};
-
-
-/**
- *  struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050
- *  slave devices
- *
- *  @type: the type of slave device based on the enum ext_slave_type
- *         definitions.
- *  @irq: the irq number attached to the slave if any.
- *  @adapt_num: the I2C adapter number.
- *  @bus: the bus the slave is attached to: enum ext_slave_bus
- *  @address: the I2C slave address of the slave device.
- *  @orientation: the mounting matrix of the device relative to MPU.
- *  @irq_data: private data for the slave irq handler
- *  @private_data: additional data, user customizable.  Not touched by the MPU
- *                 driver.
- *
- * The orientation matricies are 3x3 rotation matricies
- * that are applied to the data to rotate from the mounting orientation to the
- * platform orientation.  The values must be one of 0, 1, or -1 and each row and
- * column should have exactly 1 non-zero value.
- */
-struct ext_slave_platform_data {
-	__u8 type;
-	__u32 irq;
-	__u32 adapt_num;
-	__u32 bus;
-	__u8 address;
-	__s8 orientation[9];
-	void *irq_data;
-	void *private_data;
-};
-
-struct fix_pnt_range {
-	__s32 mantissa;
-	__s32 fraction;
-};
-
-static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
-{
-	return (long)(rng.mantissa * 1000 + rng.fraction / 10);
-}
-
-struct ext_slave_read_trigger {
-	__u8 reg;
-	__u8 value;
-};
-
-/**
- *  struct ext_slave_descr - Description of the slave device for programming.
- *
- *  @suspend:	function pointer to put the device in suspended state
- *  @resume:	function pointer to put the device in running state
- *  @read:	function that reads the device data
- *  @init:	function used to preallocate memory used by the driver
- *  @exit:	function used to free memory allocated for the driver
- *  @config:	function used to configure the device
- *  @get_config:function used to get the device's configuration
- *
- *  @name:	text name of the device
- *  @type:	device type. enum ext_slave_type
- *  @id:	enum ext_slave_id
- *  @read_reg:	starting register address to retrieve data.
- *  @read_len:	length in bytes of the sensor data.  Typically  6.
- *  @endian:	byte order of the data. enum ext_slave_endian
- *  @range:	full scale range of the slave ouput: struct fix_pnt_range
- *  @trigger:	If reading data first requires writing a register this is the
- *		data to write.
- *
- *  Defines the functions and information about the slave the mpu3050 and
- *  mpu6050 needs to use the slave device.
- */
-struct ext_slave_descr {
-	int (*init) (void *mlsl_handle,
-		     struct ext_slave_descr *slave,
-		     struct ext_slave_platform_data *pdata);
-	int (*exit) (void *mlsl_handle,
-		     struct ext_slave_descr *slave,
-		     struct ext_slave_platform_data *pdata);
-	int (*suspend) (void *mlsl_handle,
-			struct ext_slave_descr *slave,
-			struct ext_slave_platform_data *pdata);
-	int (*resume) (void *mlsl_handle,
-		       struct ext_slave_descr *slave,
-		       struct ext_slave_platform_data *pdata);
-	int (*read) (void *mlsl_handle,
-		     struct ext_slave_descr *slave,
-		     struct ext_slave_platform_data *pdata,
-		     __u8 *data);
-	int (*config) (void *mlsl_handle,
-		       struct ext_slave_descr *slave,
-		       struct ext_slave_platform_data *pdata,
-		       struct ext_slave_config *config);
-	int (*get_config) (void *mlsl_handle,
-			   struct ext_slave_descr *slave,
-			   struct ext_slave_platform_data *pdata,
-			   struct ext_slave_config *config);
-
-	char *name;
-	__u8 type;
-	__u8 id;
-	__u8 read_reg;
-	__u8 read_len;
-	__u8 endian;
-	struct fix_pnt_range range;
-	struct ext_slave_read_trigger *trigger;
-};
-
 /**
  * struct mpu_platform_data - Platform data for the mpu driver
  * @int_config:		Bits [7:3] of the int config register.
@@ -334,6 +86,7 @@ struct ext_slave_descr {
  * @sec_slave_id:       id of the secondary slave device
  * @secondary_i2c_address: secondary device's i2c address
  * @secondary_orientation: secondary device's orientation matrix
+ * @key:                key for MPL library.
  *
  * Contains platform specific information on how to configure the MPU3050 to
  * work on this platform.  The orientation matricies are 3x3 rotation matricies
diff --git a/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h
index 74c49f3..97e5b2f 100644
--- a/libsensors_iio/software/core/driver/include/log.h
+++ b/libsensors_iio/software/core/driver/include/log.h
@@ -37,8 +37,8 @@
 #ifndef _LIBS_CUTILS_MPL_LOG_H
 #define _LIBS_CUTILS_MPL_LOG_H
 
+#include <stdlib.h>
 #include <stdarg.h>
-#include "local_log_def.h"
 
 #ifdef ANDROID
 #ifdef NDK_BUILD
@@ -56,6 +56,10 @@
 extern "C" {
 #endif
 
+#if defined ANDROID_JELLYBEAN
+#define LOG ALOG
+#endif
+
 /* --------------------------------------------------------------------- */
 
 /*
@@ -287,7 +291,7 @@ extern "C" {
 #ifndef MPL_LOG_PRI
 #ifdef ANDROID
 #define MPL_LOG_PRI(priority, tag, fmt, ...) \
-	LOG(priority, tag, fmt, ##__VA_ARGS__)
+	ALOG(priority, tag, fmt, ##__VA_ARGS__)
 #elif defined __KERNEL__
 #define MPL_LOG_PRI(priority, tag, fmt, ...) \
 	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
diff --git a/libsensors_iio/software/core/mllite/CMakeLists.txt b/libsensors_iio/software/core/mllite/CMakeLists.txt
deleted file mode 100644
index 8650553..0000000
--- a/libsensors_iio/software/core/mllite/CMakeLists.txt
+++ /dev/null
@@ -1,39 +0,0 @@
-## CMakeLists for mlsdk/mldmp
-
-project(mldmp C)
-
-if (NOT CMAKE_BUILD_ENGINEERING)
-
-#    # just copy the library from mldmp/mpl/$PLATFORM to mldmp/
-#    if (CMAKE_SYSTEM_NAME MATCHES Windows)
-#        execute_process(
-#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib
-#        )
-#    elseif (CMAKE_SYSTEM_NAME MATCHES Android)
-#        execute_process(
-#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
-#        )
-#    elseif (CMAKE_SYSTEM_NAME MATCHES Linux)
-#        message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a")
-#        execute_process(
-#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
-#        )
-#    elseif (CMAKE_SYSTEM_NAME MATCHES Catalina)
-#        execute_process(
-#            COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
-#        )
-#    endif()
-#    # better way that doesn't work for now
-#    # add_custom_command(
-#        # TARGET mpl
-#        # PRE_BUILD
-#        # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR}
-#        # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}"
-#    # )
-    
-else (NOT CMAKE_BUILD_ENGINEERING)
-
-  set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR})
-  include(Engineering)
-
-endif (NOT CMAKE_BUILD_ENGINEERING)
diff --git a/libsensors_iio/software/core/mllite/Engineering.cmake b/libsensors_iio/software/core/mllite/Engineering.cmake
deleted file mode 100644
index 42f2429..0000000
--- a/libsensors_iio/software/core/mllite/Engineering.cmake
+++ /dev/null
@@ -1,150 +0,0 @@
-## Engineering CMakeLists for software/core/mllite
-
-include_directories(
-    .
-    ${SOLUTION_SOURCE_DIR}/core/mpl
-    ${SOLUTION_SOURCE_DIR}/core/HAL
-    ${SOLUTION_SOURCE_DIR}/driver/include
-)
-
-add_definitions (
-    -DINV_INCLUDE_LEGACY_HEADERS
-)
-
-if (CMAKE_SYSTEM_NAME MATCHES Android)
-
-    include_directories(
-        ${SOLUTION_SOURCE_DIR}/core/mllite/linux
-        ${SOLUTION_SOURCE_DIR}/driver/include/linux
-    )
-    add_definitions(
-        -DLINUX
-        -D_REENTRANT
-    )
-    set (HEADERS
-        ${HEADERS}
-        linux/mlos.h
-        linux/ml_stored_data.h
-        linux/ml_load_dmp.h
-        linux/ml_sysfs_helper.h
-    )
-    set (SOURCES
-        ${SOURCES}
-        linux/mlos_linux.c
-        linux/ml_stored_data.c
-        linux/ml_load_dmp.c
-        linux/ml_sysfs_helper.c
-    )
-
-elseif (CMAKE_SYSTEM_NAME MATCHES Linux)
-
-    add_definitions(
-        -DLINUX
-        -D_REENTRANT
-    )
-    set (HEADERS
-        ${HEADERS}
-    )
-    set (SOURCES
-        ${SOURCES}
-    )
-
-elseif (CMAKE_SYSTEM_NAME MATCHES Windows)
-
-    add_definitions(
-        -DAIO
-        -DUART_COM
-        -D_CRT_SECURE_NO_WARNINGS
-        -D_CRT_SECURE_NO_DEPRECATE
-    )
-    set (HEADERS
-        ${HEADERS}
-    )
-    set (SOURCES
-        ${SOURCES}
-    )
-
-endif ()
-
-set (HEADERS
-    ${HEADERS}
-    data_builder.h
-    hal_outputs.h
-    message_layer.h
-    ml_math_func.h
-    mpl.h
-    results_holder.h
-    start_manager.h
-    storage_manager.h
-)
-set (SOURCES
-    ${SOURCES}
-    data_builder.c
-    hal_outputs.c
-    message_layer.c
-    ml_math_func.c
-    mpl.c
-    results_holder.c
-    start_manager.c
-    storage_manager.c
-)
-
-# coveniently add this file to the resources for easy access within IDEs
-set (RESOURCES
-    Engineering.cmake
-)
-
-if (CMAKE_TESTING_SUPPORT)
-
-    message("Including Testing support")
-    include_directories(
-        ${SOLUTION_SOURCE_DIR}/mltools/debugsupport
-    )
-    add_definitions(
-        -DSELF_VERIFICATION
-    )
-    set(
-        HEADERS 
-        ${HEADERS}
-        ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h
-    )
-    set(
-        SOURCES
-        ${SOURCES}
-        ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c
-    )
-
-endif ()
-
-############
-## TARGET ##
-############
-add_library(
-    mllite STATIC
-    ${SOURCES}
-    ${HEADERS}
-    ${RESOURCES}
-)
-set_target_properties(
-    mllite
-    PROPERTIES CLEAN_DIRECT_OUTPUT 1
-)
-
-if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)")
-
-    add_library(
-        mllite_shared SHARED
-        ${SOURCES}
-        ${HEADERS}
-        ${RESOURCES}
-    )
-    FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite")
-
-    install (
-        TARGETS mllite_shared
-        DESTINATION lib
-    )
-
-endif () 
-
-
diff --git a/libsensors_iio/software/core/mllite/build/android/libmllite.so b/libsensors_iio/software/core/mllite/build/android/libmllite.so
new file mode 100644
index 0000000..acd25a7
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/libsensors_iio/software/core/mllite/build/android/shared.mk b/libsensors_iio/software/core/mllite/build/android/shared.mk
index 2ee2e20..1418450 100644
--- a/libsensors_iio/software/core/mllite/build/android/shared.mk
+++ b/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -15,6 +15,7 @@ MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -44,12 +45,7 @@ LLINK += -ldl
 LFLAGS += $(CMDLINE_LFLAGS)
 LFLAGS += -shared 
 LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -nostdlib 
-LFLAGS += -fpic 
-LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc 
-LFLAGS += -Wl,--gc-sections 
-LFLAGS += -Wl,--no-whole-archive 
-LFLAGS += -Wl,-shared,-Bsymbolic 
+LFLAGS += -Wl,-shared,-Bsymbolic
 LFLAGS += $(ANDROID_LINK)
 LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
diff --git a/libsensors_iio/software/core/mllite/build/android/static.mk b/libsensors_iio/software/core/mllite/build/android/static.mk
deleted file mode 100644
index 6ad45de..0000000
--- a/libsensors_iio/software/core/mllite/build/android/static.mk
+++ /dev/null
@@ -1,110 +0,0 @@
-MLLITE_LIB_NAME = mllite
-LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT)
-
-MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
-
-OBJFOLDER = $(CURDIR)/obj
-
-CROSS = arm-none-linux-gnueabi-
-COMP= $(CROSS)gcc
-LINK= $(CROSS)ar cr
-
-MLLITE_DIR = $(MLSDK_ROOT)/mllite
-MPL_DIR = $(MLSDK_ROOT)/mldmp
-MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
-
-include $(MLSDK_ROOT)/Android-common.mk
-
-CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += -Wall -fpic
-CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0
-CFLAGS += -DNDEBUG
-CFLAGS += -D_REENTRANT -DLINUX -DANDROID
-CFLAGS += -I$(MLLITE_DIR) 
-CFLAGS += -I$(MLPLATFORM_DIR)/include
-CFLAGS += -I$(MLSDK_ROOT)/mlutils 
-CFLAGS += -I$(MLSDK_ROOT)/mlapps/common
-CFLAGS += $(MLSDK_INCLUDES)
-CFLAGS += $(MLSDK_DEFINES)
-
-VPATH += $(MLLITE_DIR) 
-VPATH += $(MLSDK_ROOT)/mlutils
-VPATH += $(MLLITE_DIR)/accel 
-VPATH += $(MLLITE_DIR)/compass 
-VPATH += $(MLLITE_DIR)/pressure 
-VPATH += $(MLLITE_DIR)/mlapps/common
-
-####################################################################################################
-## sources
-
-ML_LIBS  = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT)
-
-ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c
-ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c
-ML_SOURCES += $(MLLITE_DIR)/accel.c
-ML_SOURCES += $(MLLITE_DIR)/compass.c
-ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c
-ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c
-ML_SOURCES += $(MLLITE_DIR)/key0_96.c
-ML_SOURCES += $(MLLITE_DIR)/pressure.c
-ML_SOURCES += $(MLLITE_DIR)/ml.c
-ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c
-ML_SOURCES += $(MLLITE_DIR)/ml_init.c
-ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c
-ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c
-ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c
-ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c
-ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c
-ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c
-ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c
-ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c
-ML_SOURCES += $(MLLITE_DIR)/mldl.c
-ML_SOURCES += $(MLLITE_DIR)/mldmp.c
-ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c
-ML_SOURCES += $(MLLITE_DIR)/mlstates.c
-ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c
-ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c
-ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c
-ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c
-ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c
-ifeq ($(MPU_NAME),MPU3050)
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c
-else
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c
-endif
-
-ML_OBJS := $(addsuffix .o,$(ML_SOURCES))
-ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES))))
-
-####################################################################################################
-## rules
-
-.PHONY: all clean cleanall
-
-all: $(LIBRARY) $(MK_NAME)
-
-$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME)
-	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n")
-	$(LINK) $(LIBRARY) $(ML_OBJS_DST)
-	$(CROSS)ranlib $(LIBRARY)
-
-$(OBJFOLDER) : 
-	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
-	mkdir obj
-
-$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
-	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
-	$(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $<
-
-clean : 
-	rm -fR $(OBJFOLDER)
-
-cleanall : 
-	rm -fR $(LIBRARY) $(OBJFOLDER)
-
diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c
index f70be7c..48993bc 100644
--- a/libsensors_iio/software/core/mllite/data_builder.c
+++ b/libsensors_iio/software/core/mllite/data_builder.c
@@ -18,11 +18,14 @@
 #undef MPL_LOG_NDEBUG
 #define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
 
+#include <string.h>
+
 #include "ml_math_func.h"
 #include "data_builder.h"
 #include "mlmath.h"
 #include "storage_manager.h"
 #include "message_layer.h"
+#include "results_holder.h"
 
 #include "log.h"
 #undef MPL_LOG_TAG
@@ -48,6 +51,10 @@ struct inv_db_save_t {
     /** Temperature when accel bias was stored. */
     long accel_temp;
     long gyro_temp_slope[3];
+    /** Sensor Accuracy */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
 };
 
 struct inv_data_builder_t {
@@ -69,7 +76,7 @@ static struct inv_data_builder_t inv_data_builder;
 static struct inv_sensor_cal_t sensors;
 
 /** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53394
+#define INV_DB_SAVE_KEY 53395
 
 #ifdef INV_PLAYBACK_DBG
 
@@ -98,6 +105,14 @@ void inv_turn_off_data_logging()
 static inv_error_t inv_db_load_func(const unsigned char *data)
 {
     memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
+    // copy in the saved accuracy in the actual sensors accuracy
+    sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
+    sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
+    sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
+    // TODO
+    if (sensors.compass.accuracy == 3) {
+        inv_set_compass_bias_found(1);
+    }
     return INV_SUCCESS;
 }
 
@@ -162,119 +177,7 @@ void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
     sensor->sensitivity = sensitivity;
     sensor->orientation = orientation;
 }
-void inv_get_quaternion_transformation(int orientation, long *q)
-{
-    long s = 759250125; // sqrt(.5)*2^30
-
-    switch (orientation)
-    {
-    case 0x70:
-        q[0] = 759250125;
-        q[1] = 759250125;
-        q[2] = 0;
-        q[3] = 0;
-        break;
-    case 0x10a:
-        q[0] = 759250125;
-        q[1] = 0;
-        q[2] = 759250125;
-        q[3] = 0;
-        break;
-    case 0x85:
-        q[0] = 759250125;
-        q[1] = 0;
-        q[2] = 0;
-        q[3] = 759250125;
-        break;
-    case 0x181:
-        q[0] = 0;
-        q[1] = 759250125;
-        q[2] = 759250125;
-        q[3] = 0;
-        break;
-    case 0x2a:
-        q[0] = 0;
-        q[1] = 759250125;
-        q[2] = 0;
-        q[3] = 759250125;
-        break;
-    case 0x54:
-        q[0] = 0;
-        q[1] = 0;
-        q[2] = 759250125;
-        q[3] = 759250125;
-        break;
-    case 0x150:
-        q[0] = 759250125;
-        q[1] = -759250125;
-        q[2] = 0;
-        q[3] = 0;
-        break;
-    case 0xe:
-        q[0] = 759250125;
-        q[1] = 0;
-        q[2] = -759250125;
-        q[3] = 0;
-        break;
-    case 0xa1:
-        q[0] = 759250125;
-        q[1] = 0;
-        q[2] = 0;
-        q[3] = -759250125;
-        break;
-    case 0x1a5:
-        q[0] = 0;
-        q[1] = 759250125;
-        q[2] = -759250125;
-        q[3] = 0;
-        break;
-    case 0x12e:
-        q[0] = 0;
-        q[1] = 759250125;
-        q[2] = 0;
-        q[3] = -759250125;
-        break;
-    case 0x174:
-        q[0] = 0;
-        q[1] = 0;
-        q[2] = 759250125;
-        q[3] = -759250125;
-        break;
-
-
-    case 0x88:
-        q[0] = 1073741824;
-        q[1] = 0;
-        q[2] = 0;
-        q[3] = 0;
-        break;
-
-
-    case 0x1a8:
-        q[0] = 0;
-        q[1] = 1073741824;
-        q[2] = 0;
-        q[3] = 0;
-        break;
-
-    case 0x18c:
-        q[0] = 0;
-        q[1] = 0;
-        q[2] = 1073741824;
-        q[3] = 0;
-        break;
-
-    case 0xac:
-        q[0] = 0;
-        q[1] = 0;
-        q[2] = 0;
-        q[3] = 1073741824;
-        break;
-
-    default:
-        break;
-    }
-}
+
 /** Sets the Orientation and Sensitivity of the gyro data.
 * @param[in] orientation A scalar defining the transformation from chip mounting
 *            to the body frame. The function inv_orientation_matrix_to_scalar()
@@ -354,6 +257,22 @@ void inv_set_compass_sample_rate(long sample_rate_us)
         sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
     }
 }
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.gyro.sample_rate_ms;
+}
+
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.accel.sample_rate_ms;
+}
+
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
+{
+	*sample_rate_ms = sensors.compass.sample_rate_ms;
+}
+
 /** Set Quat Sample rate in micro seconds.
 * @param[in] sample_rate_us Set Quat Sample rate in us
 */
@@ -498,7 +417,7 @@ void inv_matrix_vector_mult(const long *A, const long *x, long *y)
 }
 
 /** Takes raw data stored in the sensor, removes bias, and converts it to
-* calibrated data in the body frame.
+* calibrated data in the body frame. Also store raw data for body frame.
 * @param[in,out] sensor structure to modify
 * @param[in] bias bias in the mounting frame, in hardware units scaled by
 *                 2^16. Length 3.
@@ -508,15 +427,17 @@ void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
     long raw32[3];
 
     // Convert raw to calibrated
-    raw32[0] = (long)sensor->raw[0] << 16;
-    raw32[1] = (long)sensor->raw[1] << 16;
-    raw32[2] = (long)sensor->raw[2] << 16;
+    raw32[0] = (long)sensor->raw[0] << 15;
+    raw32[1] = (long)sensor->raw[1] << 15;
+    raw32[2] = (long)sensor->raw[2] << 15;
+
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
 
-    raw32[0] -= bias[0];
-    raw32[1] -= bias[1];
-    raw32[2] -= bias[2];
+    raw32[0] -= bias[0] >> 1;
+    raw32[1] -= bias[1] >> 1;
+    raw32[2] -= bias[2] >> 1;
 
-    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated);
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
 
     sensor->status |= INV_CALIBRATED;
 }
@@ -539,6 +460,7 @@ void inv_set_compass_bias(const long *bias, int accuracy)
         inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
     }
     sensors.compass.accuracy = accuracy;
+    inv_data_builder.save.compass_accuracy = accuracy;
     inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
 }
 
@@ -566,6 +488,17 @@ void inv_set_accel_bias(const long *bias, int accuracy)
         }
     }
     sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
+/** Sets the accel accuracy.
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_accuracy(int accuracy)
+{
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
     inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
 }
 
@@ -590,6 +523,8 @@ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
         inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
     }
     sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
 }
 
 
@@ -607,6 +542,8 @@ void inv_set_gyro_bias(const long *bias, int accuracy)
         }
     }
     sensors.gyro.accuracy = accuracy;
+    inv_data_builder.save.gyro_accuracy = accuracy;
+
     /* TODO: What should we do if there's no temperature data? */
     if (sensors.temp.calibrated[0])
         inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
@@ -687,6 +624,7 @@ inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
         sensors.accel.calibrated[2] = accel[2];
         sensors.accel.status |= INV_CALIBRATED;
         sensors.accel.accuracy = status & 3;
+        inv_data_builder.save.accel_accuracy = status & 3;
     }
     sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
     sensors.accel.timestamp_prev = sensors.accel.timestamp;
@@ -757,6 +695,7 @@ inv_error_t inv_build_compass(const long *compass, int status,
         sensors.compass.calibrated[2] = compass[2];
         sensors.compass.status |= INV_CALIBRATED;
         sensors.compass.accuracy = status & 3;
+        inv_data_builder.save.compass_accuracy = status & 3;
     }
     sensors.compass.timestamp_prev = sensors.compass.timestamp;
     sensors.compass.timestamp = timestamp;
@@ -1077,6 +1016,22 @@ void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
     }
 }
 
+/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+    memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
+    if (timestamp != NULL) {
+        *timestamp = sensors.gyro.timestamp;
+    }
+    if (accuracy != NULL) {
+        *accuracy = sensors.gyro.accuracy;
+    }
+}
+
 /** Get's latest gyro data.
 * @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
 */
diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h
index b2d0881..9aa46e6 100644
--- a/libsensors_iio/software/core/mllite/data_builder.h
+++ b/libsensors_iio/software/core/mllite/data_builder.h
@@ -56,6 +56,7 @@ extern "C" {
 #define INV_PRIORITY_INUSE_AUTO_CALIBRATION    850
 #define INV_PRIORITY_HAL_OUTPUTS               900
 #define INV_PRIORITY_GLYPH                     950
+#define INV_PRIORITY_SHAKE                     975
 #define INV_PRIORITY_SM                        1000
 
 struct inv_single_sensor_t {
@@ -69,6 +70,8 @@ struct inv_single_sensor_t {
     int orientation;
     /** The raw data in raw data units in the mounting frame */
     short raw[3];
+    /** Raw data in body frame */
+    long raw_scaled[3];
     /** Calibrated data */
     long calibrated[3];
     long sensitivity;
@@ -160,6 +163,10 @@ void inv_set_gyro_bandwidth(int bandwidth_hz);
 void inv_set_accel_bandwidth(int bandwidth_hz);
 void inv_set_compass_bandwidth(int bandwidth_hz);
 
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms);
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms);
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms);
+
 inv_error_t inv_register_data_cb(inv_error_t (*func)
                                  (struct inv_sensor_cal_t * data), int priority,
                                  int sensor_type);
@@ -181,6 +188,7 @@ void inv_set_compass_bias(const long *bias, int accuracy);
 void inv_set_compass_disturbance(int dist);
 void inv_set_gyro_bias(const long *bias, int accuracy);
 void inv_set_accel_bias(const long *bias, int accuracy);
+void inv_set_accel_accuracy(int accuracy);
 void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask);
 
 void inv_get_gyro_bias(long *bias, long *temp);
@@ -197,6 +205,7 @@ long inv_get_compass_sensitivity(void);
 
 void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
 void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
 void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
 
 void inv_get_gyro(long *gyro);
diff --git a/libsensors_iio/software/core/mllite/dmpconfig.txt b/libsensors_iio/software/core/mllite/dmpconfig.txt
deleted file mode 100644
index 4643ed5..0000000
--- a/libsensors_iio/software/core/mllite/dmpconfig.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-major version    =  1
-minor version    =  0
-startAddr        =  0
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c
index 1cd3b81..6d457db 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.c
+++ b/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -4,16 +4,19 @@
     See included License.txt for License information.
  $
  */
- 
+
 /**
  *   @defgroup  HAL_Outputs hal_outputs
  *   @brief     Motion Library - HAL Outputs
  *              Sets up common outputs for HAL
  *
  *   @{
- *       @file hal_outputs.c
+ *       @file  hal_outputs.c
  *       @brief HAL Outputs.
  */
+
+#include <string.h>
+
 #include "hal_outputs.h"
 #include "log.h"
 #include "ml_math_func.h"
@@ -23,17 +26,22 @@
 #include "results_holder.h"
 
 struct hal_output_t {
-    int accuracy_mag; /**< Compass accuracy */
-    int accuracy_gyro; /**< Gyro Accuracy */
-    int accuracy_accel;	/**< Accel Accuracy */
+    int accuracy_mag;    /**< Compass accuracy */
+//    int accuracy_gyro;   /**< Gyro Accuracy */
+//    int accuracy_accel;  /**< Accel Accuracy */
+    int accuracy_quat;   /**< quat Accuracy */
+
     inv_time_t nav_timestamp;
     inv_time_t gam_timestamp;
-    inv_time_t accel_timestamp;
+//    inv_time_t accel_timestamp;
+    inv_time_t mag_timestamp;
     long nav_quat[4];
     int gyro_status;
     int accel_status;
     int compass_status;
     int nine_axis_status;
+    inv_biquad_filter_t lp_filter[3];
+    float compass_float[3];
 };
 
 static struct hal_output_t hal_out;
@@ -103,7 +111,7 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
     long gravity[3];
     int status;
 
-    *accuracy = hal_out.accuracy_mag;
+    *accuracy = (int8_t) hal_out.accuracy_quat;
     *timestamp = hal_out.nav_timestamp;
     inv_get_gravity(gravity);
     values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
@@ -116,7 +124,11 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
     return status;
 }
 
-/** Gyroscope data (rad/s) in body frame.
+/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
+ * So this is: pi / 2^16 / 180 */
+#define GYRO_CONVERSION 2.66316109007924e-007f
+
+/** Gyroscope calibrated data (rad/s) in body frame.
 * @param[out] values Rotation Rate in rad/sec.
 * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
 * @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
@@ -126,9 +138,6 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
 int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
                                    inv_time_t * timestamp)
 {
-    /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
-     * So this is: pi / 2^16 / 180 */
-#define GYRO_CONVERSION 2.66316109007924e-007f
     long gyro[3];
     int status;
 
@@ -143,6 +152,30 @@ int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
     return status;
 }
 
+/** Gyroscope raw data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+*             inv_build_gyro().
+* @return     Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+                                   inv_time_t * timestamp)
+{
+    long gyro[3];
+    int status;
+
+    inv_get_gyro_set_raw(gyro, accuracy, timestamp);
+    values[0] = gyro[0] * GYRO_CONVERSION;
+    values[1] = gyro[1] * GYRO_CONVERSION;
+    values[2] = gyro[2] * GYRO_CONVERSION;
+    if (hal_out.gyro_status & INV_NEW_DATA)
+        status = 1;
+    else
+        status = 0;
+    return status;
+}
+
 /**
 * This corresponds to Sensor.TYPE_ROTATION_VECTOR.
 * The rotation vector represents the orientation of the device as a combination
@@ -170,7 +203,7 @@ int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
 int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
         inv_time_t * timestamp)
 {
-    *accuracy = hal_out.accuracy_mag;
+    *accuracy = (int8_t) hal_out.accuracy_quat;
     *timestamp = hal_out.nav_timestamp;
 
     if (hal_out.nav_quat[0] >= 0) {
@@ -203,19 +236,54 @@ int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
     int status;
     /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
      * So this is: 1 / 2^16*/
-#define COMPASS_CONVERSION 1.52587890625e-005f
-    long compass[3];
-    inv_get_compass_set(compass, accuracy, timestamp);
-    values[0] = compass[0] * COMPASS_CONVERSION;
-    values[1] = compass[1] * COMPASS_CONVERSION;
-    values[2] = compass[2] * COMPASS_CONVERSION;
+//#define COMPASS_CONVERSION 1.52587890625e-005f
+    int i;
+
+    *timestamp = hal_out.mag_timestamp;
+    *accuracy = (int8_t) hal_out.accuracy_mag;
+
+    for (i=0; i<3; i++)  {
+        values[i] = hal_out.compass_float[i];
+    }
     if (hal_out.compass_status & INV_NEW_DATA)
         status = 1;
     else
         status = 0;
+    hal_out.compass_status = 0;
     return status;
 }
 
+static void inv_get_rotation(float r[3][3])
+{
+    long rot[9];
+    float conv = 1.f / (1L<<30);
+
+    inv_quaternion_to_rotation(hal_out.nav_quat, rot);
+    r[0][0] = rot[0]*conv;
+    r[0][1] = rot[1]*conv;
+    r[0][2] = rot[2]*conv;
+    r[1][0] = rot[3]*conv;
+    r[1][1] = rot[4]*conv;
+    r[1][2] = rot[5]*conv;
+    r[2][0] = rot[6]*conv;
+    r[2][1] = rot[7]*conv;
+    r[2][2] = rot[8]*conv;
+}
+
+static void google_orientation(float *g)
+{
+    float rad2deg = (float)(180.0 / M_PI);
+    float R[3][3];
+
+    inv_get_rotation(R);
+
+    g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
+    g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
+    g[2] = asinf ( R[2][0])          * rad2deg;
+    if (g[0] < 0)
+        g[0] += 360;
+}
+
 
 /** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
 * @param[out] values Length 3, Degrees.<br>
@@ -241,78 +309,29 @@ int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
 int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
                                      inv_time_t * timestamp)
 {
-    long t1, t2, t3;
-    long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
-
-    *accuracy = hal_out.accuracy_mag;
+    *accuracy = (int8_t) hal_out.accuracy_quat;
     *timestamp = hal_out.nav_timestamp;
 
-    q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]);
-    q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]);
-    q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]);
-    q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]);
-    q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]);
-    q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]);
-    q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]);
-    q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]);
-    q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]);
-    q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]);
-
-    /* X component of the Ybody axis in World frame */
-    t1 = q12 - q03;
-
-    /* Y component of the Ybody axis in World frame */
-    t2 = q22 + q00 - (1L << 30);
-    values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
-    if (values[0] < 0)
-        values[0] += 360;
-
-    /* Z component of the Ybody axis in World frame */
-    t3 = q23 + q01;
-    values[1] =
-        -atan2f((float) t3,
-                sqrtf((float) t1 * t1 +
-                      (float) t2 * t2)) * 180.f / (float) M_PI;
-    /* Z component of the Zbody axis in World frame */
-    t2 = q33 + q00 - (1L << 30);
-    if (t2 < 0) {
-        if (values[1] >= 0)
-            values[1] = 180.f - values[1];
-        else
-            values[1] = -180.f - values[1];
-    }
-
-    /* X component of the Xbody axis in World frame */
-    t1 = q11 + q00 - (1L << 30);
-    /* Y component of the Xbody axis in World frame */
-    t2 = q12 + q03;
-    /* Z component of the Xbody axis in World frame */
-    t3 = q13 - q02;
-    //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI;
-
-    values[2] =
-        -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
-          180.f / (float) M_PI - 90);
-    if (values[2] >= 90)
-        values[2] = 180 - values[2];
-
-    if (values[2] < -90)
-        values[2] = -180 - values[2];
+    google_orientation(values);
 
     return hal_out.nine_axis_status;
 }
 
 /** Main callback to generate HAL outputs. Typically not called by library users.
-* @param[in] sensor_cal Input variable to take sensor data whenever there is new 
+* @param[in] sensor_cal Input variable to take sensor data whenever there is new
 * sensor data.
 * @return Returns INV_SUCCESS if successful or an error code if not.
 */
 inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
 {
     int use_sensor = 0;
-    long sr;
+    long sr = 1000;
+    long compass[3];
+    int8_t accuracy;
+    int i;
     (void) sensor_cal;
-    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag,
+
+    inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
                            &hal_out.nav_timestamp);
     hal_out.gyro_status = sensor_cal->gyro.status;
     hal_out.accel_status = sensor_cal->accel.status;
@@ -336,26 +355,65 @@ inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
         use_sensor = 3;
     }
 
+    // Only output 9-axis if all 9 sensors are on.
+    if (sensor_cal->quat.status & INV_SENSOR_ON) {
+        // If quaternion sensor is on, gyros are not required as quaternion already has that part
+        if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+            use_sensor = -1;
+        }
+    } else {
+        if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+            use_sensor = -1;
+        }
+    }
+
     switch (use_sensor) {
-    default:
     case 0:
         hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
-        hal_out.nav_timestamp = sensor_cal->gyro.timestamp; 
+        hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
         break;
     case 1:
         hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
-        hal_out.nav_timestamp = sensor_cal->accel.timestamp; 
+        hal_out.nav_timestamp = sensor_cal->accel.timestamp;
         break;
     case 2:
         hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
-        hal_out.nav_timestamp = sensor_cal->compass.timestamp; 
+        hal_out.nav_timestamp = sensor_cal->compass.timestamp;
         break;
     case 3:
         hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
-        hal_out.nav_timestamp = sensor_cal->quat.timestamp; 
+        hal_out.nav_timestamp = sensor_cal->quat.timestamp;
+        break;
+    default:
+        hal_out.nine_axis_status = 0; // Don't output quaternion related info
         break;
     }
 
+    /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+     * So this is: 1 / 2^16*/
+    #define COMPASS_CONVERSION 1.52587890625e-005f
+
+    inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
+    hal_out.accuracy_mag = (int ) accuracy;
+
+    for (i=0; i<3; i++) {
+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
+                                                             INV_NEW_DATA )  {
+            // set the state variables to match output with input
+            inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);
+        }
+
+        if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
+                                         (INV_NEW_DATA | INV_RAW_DATA)   )  {
+
+            hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],
+                                           (float ) compass[i]) * COMPASS_CONVERSION;
+
+        } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA )  {
+            hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;
+        }
+
+    }
     return INV_SUCCESS;
 }
 
@@ -382,6 +440,9 @@ inv_error_t inv_start_hal_outputs(void)
                              INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
     return result;
 }
+/* file name: lowPassFilterCoeff_1_6.c */
+float compass_low_pass_filter_coeff[5] =
+{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
 
 /** Initializes hal outputs class. This is called automatically by the
 * enable function. It may be called any time the feature is enabled, but
@@ -390,7 +451,12 @@ inv_error_t inv_start_hal_outputs(void)
 */
 inv_error_t inv_init_hal_outputs(void)
 {
+    int i;
     memset(&hal_out, 0, sizeof(hal_out));
+    for (i=0; i<3; i++)  {
+        inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
+    }
+
     return INV_SUCCESS;
 }
 
@@ -400,8 +466,8 @@ inv_error_t inv_init_hal_outputs(void)
 inv_error_t inv_enable_hal_outputs(void)
 {
     inv_error_t result;
-    
-	// don't need to check the result for inv_init_hal_outputs 
+
+	// don't need to check the result for inv_init_hal_outputs
 	// since it's always INV_SUCCESS
 	inv_init_hal_outputs();
 
@@ -415,7 +481,7 @@ inv_error_t inv_disable_hal_outputs(void)
 {
     inv_error_t result;
 
-    inv_stop_hal_outputs(); // Ignore error if we have already stopped this    
+    inv_stop_hal_outputs(); // Ignore error if we have already stopped this
     result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
     return result;
 }
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.h b/libsensors_iio/software/core/mllite/hal_outputs.h
index ed4cb65..85e88f3 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.h
+++ b/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -19,6 +19,8 @@ extern "C" {
                                            inv_time_t * timestamp);
     int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
                                        inv_time_t * timestamp);
+    int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+                                       inv_time_t * timestamp);
     int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
                                             inv_time_t * timestamp);
     int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
index f0f078c..bdf350f 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -30,10 +30,10 @@
 #define LOADDMP_LOG MPL_LOGI
 
 #define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
-#define DMP_CODE_SIZE 3060
+#define DMP_CODE_SIZE 3065
 
 static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
-    /* bank # 0 */
+   /* bank # 0 */
     0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 
     0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 
     0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 
@@ -69,15 +69,15 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 
     /* bank # 2 */
     0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x44, 0x00, 0x05, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 
     0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 
     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0xc0, 0x00, 0x00, 0x01, 0x3f, 0xff, 0xff, 0xff, 0x40, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, 
     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 
@@ -106,9 +106,9 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 
     0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 
     0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, 
-    0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, 0xca, 0xf1, 
-    0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, 0x99, 0x2c, 
-    0x54, 0x7c, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 
+    0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, 
+    0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 
+    0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 
     0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 
     0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 
     0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, 
@@ -212,32 +212,32 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 
     0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 
     0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, 
-    0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x8b, 0x93, 0xf2, 0x02, 0x02, 0xd1, 0xab, 0xda, 
-    0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, 
-    0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 
-    0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
-    0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 
-    0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 
-    0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, 
-    0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 0x2c, 0x87, 
-    0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, 
+    0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, 
+    0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 
+    0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 
+    0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
+    0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 
+    0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 
+    0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 
+    0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 
+    0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 
     /* bank # 11 */
-    0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28, 
-    0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2, 
-    0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 0x80, 0xd9, 
-    0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 0x2c, 0xd8, 
-    0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c, 
-    0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 0x0c, 0xd8, 
-    0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 0xff, 0xd8, 
-    0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8, 
-    0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x87, 0x31, 
-    0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 
-    0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 0xf1, 0xa6, 
-    0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 
-    0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c, 
-    0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde, 
-    0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9, 
-    0x00, 0xd8, 0xf1, 0xff
+    0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 
+    0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 
+    0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 
+    0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 
+    0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 
+    0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 
+    0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 
+    0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 
+    0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 
+    0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 
+    0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 
+    0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 
+    0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 
+    0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 
+    0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 
+    0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
 };
 
 #define DMP_VERSION (dmpMemory)
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
index c5cf2e6..b4c4249 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -21,13 +21,12 @@
 
 #include <stdio.h>
 
+#include "log.h"
 #undef MPL_LOG_TAG
 #define MPL_LOG_TAG "MPL-storeload"
 
-
 #include "ml_stored_data.h"
 #include "storage_manager.h"
-#include "log.h"
 #include "mlos.h"
 
 #define LOADCAL_DEBUG    0
@@ -38,26 +37,40 @@
 #define STORECAL_LOG MPL_LOGI
 #define LOADCAL_LOG  MPL_LOGI
 
-inv_error_t inv_read_cal(unsigned char *cal, size_t len)
+inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
 {
     FILE *fp;
-    int bytesRead;
     inv_error_t result = INV_SUCCESS;
+    size_t fsize;
 
     fp = fopen(MLCAL_FILE,"rb");
     if (fp == NULL) {
         MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
         return INV_ERROR_FILE_OPEN;
     }
-    bytesRead = fread(cal, 1, len, fp);
-    if (bytesRead != len) {
-        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
-                 bytesRead, len);
+
+    // obtain file size
+    fseek (fp, 0 , SEEK_END);
+    fsize = ftell (fp);
+    rewind (fp);
+  
+    *calData = (unsigned char *)inv_malloc(fsize);
+    if (*calData==NULL) {
+        MPL_LOGE("Could not allocate buffer of %d bytes - "
+                 "aborting\n", fsize);
+        fclose(fp);
+        return INV_ERROR_MEMORY_EXAUSTED;
+    }
+
+    *bytesRead = fread(*calData, 1, fsize, fp);
+    if (*bytesRead != fsize) {
+        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
+                 *bytesRead, fsize);
         result = INV_ERROR_FILE_READ;
         goto read_cal_end;
     }
     else {
-        MPL_LOGI("Bytes read = %d", bytesRead);
+        MPL_LOGI("Bytes read = %d", *bytesRead);
     }
 
 read_cal_end:
@@ -261,31 +274,18 @@ inv_error_t inv_store_cal(unsigned char *calData, size_t length)
  */
 inv_error_t inv_load_calibration(void)
 {
-    unsigned char *calData;
+    unsigned char *calData= NULL;
     inv_error_t result = 0;
-    size_t length;
-
-    inv_get_mpl_state_size(&length);
-    if (length <= 0) {
-        MPL_LOGE("Could not get file calibration length - "
-                 "error %d - aborting\n", result);
-        return result;
-    }
+    size_t bytesRead = 0;
 
-    calData = (unsigned char *)inv_malloc(length);
-    if (!calData) {
-        MPL_LOGE("Could not allocate buffer of %d bytes - "
-                 "aborting\n", length);
-        return INV_ERROR_MEMORY_EXAUSTED;
-    }
-
-    result = inv_read_cal(calData, length);
+    result = inv_read_cal(&calData, &bytesRead);
     if(result != INV_SUCCESS) {
         MPL_LOGE("Could not load cal file - "
                  "aborting\n");
+        goto free_mem_n_exit;
     }
 
-    result = inv_load_mpl_states(calData, length);
+    result = inv_load_mpl_states(calData, bytesRead);
     if (result != INV_SUCCESS) {
         MPL_LOGE("Could not load the calibration data - "
                  "error %d - aborting\n", result);
@@ -294,7 +294,7 @@ inv_error_t inv_load_calibration(void)
 
 free_mem_n_exit:
     inv_free(calData);
-    return INV_SUCCESS;
+    return result;
 }
 
 /**
@@ -345,7 +345,7 @@ inv_error_t inv_store_calibration(void)
 
 free_mem_n_exit:
     inv_free(calData);
-    return INV_SUCCESS;
+    return result;
 }
 
 /**
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
index 336f081..115b34c 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
+++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
@@ -36,7 +36,7 @@ inv_error_t inv_store_calibration(void);
 /*
     Internal APIs
 */
-inv_error_t inv_read_cal(unsigned char *cal, size_t len);
+inv_error_t inv_read_cal(unsigned char **, size_t *);
 inv_error_t inv_write_cal(unsigned char *cal, size_t len);
 inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len);
 inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len);
diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
index 5636a02..eca57c3 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -5,7 +5,6 @@
 #include <ctype.h>
 #define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu"
 
-#define CHIP_NUM 4
 enum PROC_SYSFS_CMD {
 	CMD_GET_SYSFS_PATH,
 	CMD_GET_DMP_PATH,
@@ -15,7 +14,13 @@ enum PROC_SYSFS_CMD {
 	CMD_GET_DEVICE_NODE
 };
 static char sysfs_path[100];
-static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"};
+static char *chip_name[] = {
+    "ITG3500", 
+    "MPU6050", 
+    "MPU9150", 
+    "MPU3050", 
+    "MPU6500"
+};
 static int chip_ind;
 static int initialized =0;
 static int status = 0;
@@ -27,6 +32,8 @@ static int iio_dev_num = 0;
 #define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
 #define FORMAT_TYPE_FILE "%s_type"
 
+#define CHIP_NUM ARRAY_SIZE(chip_name)
+
 static const char *iio_dir = "/sys/bus/iio/devices/";
 
 /**
@@ -93,7 +100,7 @@ int find_type_by_name(const char *name, const char *type)
  */
 static int parsing_proc_input(int mode, char *name){
 	const char input[] = "/proc/bus/input/devices";
-	char line[100], d;
+	char line[4096], d;
 	char tmp[100];
 	FILE *fp;
 	int i, j, result, find_flag;
@@ -394,7 +401,7 @@ inv_error_t  inv_get_input_number(const char *name, int *num)
  */
 inv_error_t inv_get_iio_trigger_path(const char *name)
 {
-	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0)
+	if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0)
 		return INV_ERROR_NOT_OPENED;
 	else
 		return INV_SUCCESS;
@@ -409,7 +416,7 @@ inv_error_t inv_get_iio_trigger_path(const char *name)
  */
 inv_error_t inv_get_iio_device_node(const char *name)
 {
-	if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0)
+	if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0)
 		return INV_ERROR_NOT_OPENED;
 	else
 		return INV_SUCCESS;
diff --git a/libsensors_iio/software/core/mllite/linux/mlos.h b/libsensors_iio/software/core/mllite/linux/mlos.h
index 287025f..d4f8912 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos.h
+++ b/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -10,6 +10,7 @@
 #ifndef __KERNEL__
 #include <stdio.h>
 #endif
+#include <pthread.h>
 
 #include "mltypes.h"
 
@@ -18,7 +19,7 @@ extern "C" {
 #endif
 
 #if defined(LINUX) || defined(__KERNEL__)
-typedef unsigned int HANDLE;
+typedef pthread_mutex_t* HANDLE;
 #endif
 
 	/* ------------ */
diff --git a/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
index 75f062e..5424508 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos_linux.c
+++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -1,13 +1,14 @@
 /*
  $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
  $
  */
+
 /*******************************************************************************
  *
  * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
  *
- *******************************************************************************/
+ ******************************************************************************/
 
 /**
  *  @defgroup MLOS
@@ -16,7 +17,7 @@
  *  @{
  *      @file mlos.c
  *      @brief OS Interface.
-**/
+ */
 
 /* ------------- */
 /* - Includes. - */
@@ -26,11 +27,10 @@
 #include <unistd.h>
 #include <pthread.h>
 #include <stdlib.h>
+#include <errno.h>
 
 #include "stdint_invensense.h"
-
 #include "mlos.h"
-#include <errno.h>
 
 
 /* -------------- */
@@ -39,14 +39,14 @@
 
 /**
  *  @brief  Allocate space
- *  @param  numBytes  number of bytes
+ *  @param  num_bytes  number of bytes
  *  @return pointer to allocated space
-**/
-void *inv_malloc(unsigned int numBytes)
+ */
+void *inv_malloc(unsigned int num_bytes)
 {
     // Allocate space.
-    void *allocPtr = malloc(numBytes);
-    return allocPtr;
+    void *alloc_ptr = malloc(num_bytes);
+    return alloc_ptr;
 }
 
 
@@ -54,12 +54,11 @@ void *inv_malloc(unsigned int numBytes)
  *  @brief  Free allocated space
  *  @param  ptr pointer to space to deallocate
  *  @return error code.
-**/
+ */
 inv_error_t inv_free(void *ptr)
 {
-    // Deallocate space.
-    free(ptr);
-
+    if (ptr)
+        free(ptr);
     return INV_SUCCESS;
 }
 
@@ -96,10 +95,10 @@ inv_error_t inv_create_mutex(HANDLE *mutex)
 inv_error_t inv_lock_mutex(HANDLE mutex)
 {
     int res;
-    pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
 
     res = pthread_mutex_lock(pm);
-    if(res == -1) 
+    if(res == -1)
         return INV_ERROR_OS_LOCK_FAILED;
 
     return INV_SUCCESS;
@@ -110,11 +109,11 @@ inv_error_t inv_lock_mutex(HANDLE mutex)
  *  @brief  Mutex unlock function
  *  @param  mutex   mutex handle
  *  @return error code.
-**/
+ */
 inv_error_t inv_unlock_mutex(HANDLE mutex)
 {
     int res;
-    pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
 
     res = pthread_mutex_unlock(pm);
     if(res == -1) 
@@ -131,7 +130,7 @@ inv_error_t inv_unlock_mutex(HANDLE mutex)
  */
 FILE *inv_fopen(char *filename)
 {
-    FILE *fp = fopen(filename,"r");
+    FILE *fp = fopen(filename, "r");
     return fp;
 }
 
@@ -154,22 +153,21 @@ void inv_fclose(FILE *fp)
 inv_error_t inv_destroy_mutex(HANDLE handle)
 {
     int error;
-    pthread_mutex_t *pm = (pthread_mutex_t*)handle;
+    pthread_mutex_t *pm = (pthread_mutex_t *)handle;
     error = pthread_mutex_destroy(pm);
-    if (error) {
+    if (error)
         return errno;
-    }
     free((void*) handle);
-    
+
     return INV_SUCCESS;}
 
 
 /**
  *  @brief  Sleep function.
  */
-void inv_sleep(int mSecs)
+void inv_sleep(int m_secs)
 {
-    usleep(mSecs*1000);
+    usleep(m_secs * 1000);
 }
 
 
@@ -182,13 +180,11 @@ unsigned long inv_get_tick_count()
 {
     struct timeval tv;
 
-    if (gettimeofday(&tv, NULL) !=0)
+    if (gettimeofday(&tv, NULL) != 0)
         return 0;
 
     return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
 }
 
-  /**********************/
- /** @} */ /* defgroup */
-/**********************/
+/** @} */
 
diff --git a/libsensors_iio/software/core/mllite/message_layer.c b/libsensors_iio/software/core/mllite/message_layer.c
index 8317957..d266d80 100644
--- a/libsensors_iio/software/core/mllite/message_layer.c
+++ b/libsensors_iio/software/core/mllite/message_layer.c
@@ -1,59 +1,59 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-/**

- *   @defgroup  Message_Layer message_layer

- *   @brief     Motion Library - Message Layer

- *              Holds Low Occurance messages

- *

- *   @{

- *       @file message_layer.c

- *       @brief Holds Low Occurance Messages.

- */

-#include "message_layer.h"

-#include "log.h"

-

-struct message_holder_t {

-    long message;

-};

-

-static struct message_holder_t mh;

-

-/** Sets a message.

-* @param[in] set The flags to set.

-* @param[in] clear Before setting anything this will clear these messages,

-*                  which is useful for mutually exclusive messages such

-*                  a motion or no motion message.

-* @param[in] level Level of the messages. It starts at 0, and may increase

-*            in the future to allow more messages if the bit storage runs out.

-*/

-void inv_set_message(long set, long clear, int level)

-{

-    if (level == 0) {

-        mh.message &= ~clear;

-        mh.message |= set;

-    }

-}

-

-/** Returns Message Flags for Level 0 Messages.

-* Levels are to allow expansion of more messages in the future.

-* @param[in] clear If set, will clear the message. Typically this will be set

-*  for one reader, so that you don't get the same message over and over.

-* @return bit field to corresponding message.

-*/

-long inv_get_message_level_0(int clear)

-{

-    long msg;

-    msg = mh.message;

-    if (clear) {

-        mh.message = 0;

-    }

-    return msg;

-}

-

-/**

- * @}

- */

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  Message_Layer message_layer
+ *   @brief     Motion Library - Message Layer
+ *              Holds Low Occurance messages
+ *
+ *   @{
+ *       @file message_layer.c
+ *       @brief Holds Low Occurance Messages.
+ */
+#include "message_layer.h"
+#include "log.h"
+
+struct message_holder_t {
+    long message;
+};
+
+static struct message_holder_t mh;
+
+/** Sets a message.
+* @param[in] set The flags to set.
+* @param[in] clear Before setting anything this will clear these messages,
+*                  which is useful for mutually exclusive messages such
+*                  a motion or no motion message.
+* @param[in] level Level of the messages. It starts at 0, and may increase
+*            in the future to allow more messages if the bit storage runs out.
+*/
+void inv_set_message(long set, long clear, int level)
+{
+    if (level == 0) {
+        mh.message &= ~clear;
+        mh.message |= set;
+    }
+}
+
+/** Returns Message Flags for Level 0 Messages.
+* Levels are to allow expansion of more messages in the future.
+* @param[in] clear If set, will clear the message. Typically this will be set
+*  for one reader, so that you don't get the same message over and over.
+* @return bit field to corresponding message.
+*/
+long inv_get_message_level_0(int clear)
+{
+    long msg;
+    msg = mh.message;
+    if (clear) {
+        mh.message = 0;
+    }
+    return msg;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/message_layer.h b/libsensors_iio/software/core/mllite/message_layer.h
index df0c0e2..e6d90d5 100644
--- a/libsensors_iio/software/core/mllite/message_layer.h
+++ b/libsensors_iio/software/core/mllite/message_layer.h
@@ -1,35 +1,35 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-#ifndef INV_MESSAGE_LAYER_H__

-#define INV_MESSAGE_LAYER_H__

-

-#include "mltypes.h"

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-

-    /* Level 0 Type Messages */

-    /** A motion event has occured */

-#define INV_MSG_MOTION_EVENT    (0x01)

-    /** A no motion event has occured */

-#define INV_MSG_NO_MOTION_EVENT (0x02)

-    /** A setting of the gyro bias has occured */

-#define INV_MSG_NEW_GB_EVENT    (0x04)

-    /** A setting of the compass bias has occured */

-#define INV_MSG_NEW_CB_EVENT    (0x08)

-    /** A setting of the accel bias has occured */

-#define INV_MSG_NEW_AB_EVENT    (0x10)

-

-    void inv_set_message(long set, long clear, int level);

-    long inv_get_message_level_0(int clear);

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif  // INV_MESSAGE_LAYER_H__

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_MESSAGE_LAYER_H__
+#define INV_MESSAGE_LAYER_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    /* Level 0 Type Messages */
+    /** A motion event has occured */
+#define INV_MSG_MOTION_EVENT    (0x01)
+    /** A no motion event has occured */
+#define INV_MSG_NO_MOTION_EVENT (0x02)
+    /** A setting of the gyro bias has occured */
+#define INV_MSG_NEW_GB_EVENT    (0x04)
+    /** A setting of the compass bias has occured */
+#define INV_MSG_NEW_CB_EVENT    (0x08)
+    /** A setting of the accel bias has occured */
+#define INV_MSG_NEW_AB_EVENT    (0x10)
+
+    void inv_set_message(long set, long clear, int level);
+    long inv_get_message_level_0(int clear);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // INV_MESSAGE_LAYER_H__
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c
index 86c4b41..cce0381 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.c
+++ b/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -655,6 +655,52 @@ double inv_vector_norm(const float *x)
 {
     return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
 }
+
+void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
+    int i;
+    // initial state to zero
+    pFilter->state[0] = 0;
+    pFilter->state[1] = 0;
+
+    // set up coefficients
+    for (i=0; i<5; i++) {
+        pFilter->c[i] = pBiquadCoeff[i];
+    }
+}
+
+void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
+{
+    pFilter->input = input;
+    pFilter->output = input;
+    pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
+    pFilter->state[1] = pFilter->state[0];
+}
+
+float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input)  {
+    float stateZero;
+
+    pFilter->input = input;
+    // calculate the new state;
+    stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0]
+                               - pFilter->c[3]*pFilter->state[1];
+
+    pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0]
+                                + pFilter->c[1]*pFilter->state[1];
+
+    // update the output and state
+    pFilter->output = pFilter->output * pFilter->c[4];
+    pFilter->state[1] = pFilter->state[0];
+    pFilter->state[0] = stateZero;
+    return pFilter->output;
+}
+
+void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3])  {
+
+    cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
+    cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
+    cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
+}
+
 /**
  * @}
  */
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.h b/libsensors_iio/software/core/mllite/ml_math_func.h
index 916de0a..535d849 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.h
+++ b/libsensors_iio/software/core/mllite/ml_math_func.h
@@ -24,6 +24,13 @@
 extern "C" {
 #endif
 
+     typedef struct {
+        float state[4];
+        float c[5];
+        float input;
+        float output;
+    }   inv_biquad_filter_t;
+
     static inline float inv_q30_to_float(long q30)
     {
         return (float) q30 / ((float)(1L << 30));
@@ -102,6 +109,11 @@ extern "C" {
     double quaternion_to_rotation_angle(const long *quat);
     double inv_vector_norm(const float *x);
 
+    void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff);
+    float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input);
+    void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input);
+    void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libsensors_iio/software/core/mllite/mlmath.c b/libsensors_iio/software/core/mllite/mlmath.c
deleted file mode 100644
index 5eb4264..0000000
--- a/libsensors_iio/software/core/mllite/mlmath.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
-    See included License.txt for License information.
- $
- */
-/*******************************************************************************
- *
- * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#include <math.h>
-
-double ml_asin(double x)
-{
-    return asin(x);
-}
-
-double ml_atan(double x)
-{
-    return atan(x);
-}
-
-double ml_atan2(double x, double y)
-{
-    return atan2(x, y);
-}
-
-double ml_log(double x)
-{
-    return log(x);
-}
-
-double ml_sqrt(double x)
-{
-    return sqrt(x);
-}
-
-double ml_ceil(double x)
-{
-    return ceil(x);
-}
-
-double ml_floor(double x)
-{
-    return floor(x);
-}
-
-double ml_cos(double x)
-{
-    return cos(x);
-}
-
-double ml_sin(double x)
-{
-    return sin(x);
-}
-
-double ml_acos(double x)
-{
-    return acos(x);
-}
-
-double ml_pow(double x, double y)
-{
-    return pow(x, y);
-}
diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c
index 231cbfd..ec87fc0 100644
--- a/libsensors_iio/software/core/mllite/mpl.c
+++ b/libsensors_iio/software/core/mllite/mpl.c
@@ -1,72 +1,72 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-/**

- *   @defgroup  MPL mpl

- *   @brief     Motion Library - Start Point

- *              Initializes MPL.

- *

- *   @{

- *       @file  mpl.c

- *       @brief MPL start point.

- */

-

-#include "storage_manager.h"

-#include "log.h"

-#include "mpl.h"

-#include "start_manager.h"

-#include "data_builder.h"

-#include "results_holder.h"

-#include "mlinclude.h"

-

-/**

- * @brief  Initializes the MPL. Should be called first and once 

- * @return Returns INV_SUCCESS if successful or an error code if not.

- */

-inv_error_t inv_init_mpl(void)

-{

-    inv_init_storage_manager();

-

-    /* initialize the start callback manager */

-    INV_ERROR_CHECK(inv_init_start_manager());

-

-    /* initialize the data builder */

-    INV_ERROR_CHECK(inv_init_data_builder());

-

-    INV_ERROR_CHECK(inv_enable_results_holder());

-

-    return INV_SUCCESS;

-}

-

-const char ml_ver[] = "InvenSense MPL 5.0.1";

-

-/**

- *  @brief  used to get the MPL version.

- *  @param  version     a string where the MPL version gets stored.

- *  @return INV_SUCCESS if successful or a non-zero error code otherwise.

- */

-inv_error_t inv_get_version(char **version)

-{

-    INVENSENSE_FUNC_START;

-    /* cast out the const */

-    *version = (char *)&ml_ver;

-    return INV_SUCCESS;

-}

-

-/** 

- *  @brief  Starts the MPL. Typically called after inv_init_mpl() or after a

- *          inv_stop_mpl() to start the MPL back up an running.

- *  @return INV_SUCCESS if successful or a non-zero error code otherwise.

- */

-inv_error_t inv_start_mpl(void)

-{

-    INV_ERROR_CHECK(inv_execute_mpl_start_notification());

-    return INV_SUCCESS;

-}

-

-/**

- * @}

- */

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+/**
+ *   @defgroup  MPL mpl
+ *   @brief     Motion Library - Start Point
+ *              Initializes MPL.
+ *
+ *   @{
+ *       @file  mpl.c
+ *       @brief MPL start point.
+ */
+
+#include "storage_manager.h"
+#include "log.h"
+#include "mpl.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+#include "mlinclude.h"
+
+/**
+ * @brief  Initializes the MPL. Should be called first and once 
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_init_mpl(void)
+{
+    inv_init_storage_manager();
+
+    /* initialize the start callback manager */
+    INV_ERROR_CHECK(inv_init_start_manager());
+
+    /* initialize the data builder */
+    INV_ERROR_CHECK(inv_init_data_builder());
+
+    INV_ERROR_CHECK(inv_enable_results_holder());
+
+    return INV_SUCCESS;
+}
+
+const char ml_ver[] = "InvenSense MPL 5.1.2";
+
+/**
+ *  @brief  used to get the MPL version.
+ *  @param  version     a string where the MPL version gets stored.
+ *  @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_get_version(char **version)
+{
+    INVENSENSE_FUNC_START;
+    /* cast out the const */
+    *version = (char *)&ml_ver;
+    return INV_SUCCESS;
+}
+
+/** 
+ *  @brief  Starts the MPL. Typically called after inv_init_mpl() or after a
+ *          inv_stop_mpl() to start the MPL back up an running.
+ *  @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_start_mpl(void)
+{
+    INV_ERROR_CHECK(inv_execute_mpl_start_notification());
+    return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c
index 97ffdec..df58f40 100644
--- a/libsensors_iio/software/core/mllite/results_holder.c
+++ b/libsensors_iio/software/core/mllite/results_holder.c
@@ -13,13 +13,16 @@
  *       @file results_holder.c
  *       @brief Results Holder for HAL.
  */
+
+#include <string.h>
+
 #include "results_holder.h"
-#include "log.h"
 #include "ml_math_func.h"
 #include "mlmath.h"
 #include "start_manager.h"
 #include "data_builder.h"
 #include "message_layer.h"
+#include "log.h"
 
 // These 2 status bits are used to control when the 9 axis quaternion is updated
 #define INV_COMPASS_CORRECTION_SET 1
@@ -34,6 +37,7 @@ struct results_t {
     long mag_scale[3]; /**< scale factor to apply to magnetic field reading */
     long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */
     int acc_state; /**< Describes accel state */
+    int got_accel_bias; /**< Flag describing if accel bias is known */
     long compass_bias_error[3]; /**< Error Squared */
     unsigned char motion_state;
     unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */
@@ -318,6 +322,24 @@ inv_error_t inv_enable_results_holder()
     return result;
 }
 
+/** Sets state of if we know the accel bias.
+ * @return return 1 if we know the accel bias, 0 if not.
+ *            it is set with inv_set_accel_bias_found()
+ */
+int inv_got_accel_bias()
+{
+    return rh.got_accel_bias;
+}
+
+/** Sets whether we know the accel bias
+ * @param[in] state Set to 1 if we know the accel bias. 
+ *            Can be retrieved with inv_got_accel_bias()
+ */
+void inv_set_accel_bias_found(int state)
+{
+    rh.got_accel_bias = state;
+}
+
 /** Sets state of if we know the compass bias.
  * @return return 1 if we know the compass bias, 0 if not.
  *            it is set with inv_set_compass_bias_found()
diff --git a/libsensors_iio/software/core/mllite/results_holder.h b/libsensors_iio/software/core/mllite/results_holder.h
index a60d7f0..09da24e 100644
--- a/libsensors_iio/software/core/mllite/results_holder.h
+++ b/libsensors_iio/software/core/mllite/results_holder.h
@@ -70,6 +70,10 @@ inv_error_t inv_get_linear_accel_float(float *data);
 void inv_set_heading_confidence_interval(float ci);
 float inv_get_heading_confidence_interval(void);
 
+int inv_got_accel_bias();
+void inv_set_accel_bias_found(int state);
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libsensors_iio/software/core/mllite/start_manager.c b/libsensors_iio/software/core/mllite/start_manager.c
index fb758e7..3e082d4 100644
--- a/libsensors_iio/software/core/mllite/start_manager.c
+++ b/libsensors_iio/software/core/mllite/start_manager.c
@@ -1,105 +1,105 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-

-/*******************************************************************************

- *

- * $Id:$

- *

- ******************************************************************************/

-/**

- *   @defgroup  Start_Manager start_manager

- *   @brief     Motion Library - Start Manager

- *              Start Manager

- *

- *   @{

- *       @file start_manager.c

- *       @brief This handles all the callbacks when inv_start_mpl() is called.

- */

-

-

-#include <string.h>

-#include "log.h"

-#include "start_manager.h"

-

-typedef inv_error_t (*inv_start_cb_func)();

-struct inv_start_cb_t {

-    int num_cb;

-    inv_start_cb_func start_cb[INV_MAX_START_CB];

-};

-

-static struct inv_start_cb_t inv_start_cb;

-

-/** Initilize the start manager. Typically called by inv_start_mpl();

-* @return Returns INV_SUCCESS if successful or an error code if not.

-*/

-inv_error_t inv_init_start_manager(void)

-{

-    memset(&inv_start_cb, 0, sizeof(inv_start_cb));

-    return INV_SUCCESS;

-}

-

-/** Removes a callback from start notification

-* @param[in] start_cb function to remove from start notification

-* @return Returns INV_SUCCESS if successful or an error code if not.

-*/

-inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))

-{

-    int kk;

-

-    for (kk=0; kk<inv_start_cb.num_cb; ++kk) {

-        if (inv_start_cb.start_cb[kk] == start_cb) {

-            // Found the match

-            if (kk != (inv_start_cb.num_cb-1)) {

-                memmove(&inv_start_cb.start_cb[kk],

-                    &inv_start_cb.start_cb[kk+1],

-                    (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));

-            }

-            inv_start_cb.num_cb--;

-            return INV_SUCCESS;

-        }

-    }

-    return INV_ERROR_INVALID_PARAMETER;

-}

-

-/** Register a callback to receive when inv_start_mpl() is called.

-* @param[in] start_cb Function callback that will be called when inv_start_mpl() is

-*            called.

-* @return Returns INV_SUCCESS if successful or an error code if not.

-*/

-inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))

-{

-    if (inv_start_cb.num_cb >= INV_MAX_START_CB)

-        return INV_ERROR_INVALID_PARAMETER;

-

-    inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;

-    inv_start_cb.num_cb++;

-    return INV_SUCCESS;

-}

-

-/** Callback all the functions that want to be notified when inv_start_mpl() was

-* called.

-* @return Returns INV_SUCCESS if successful or an error code if not.

-*/

-inv_error_t inv_execute_mpl_start_notification(void)

-{

-    inv_error_t result,first_error;

-    int kk;

-

-    first_error = INV_SUCCESS;

-

-    for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {

-        result = inv_start_cb.start_cb[kk]();

-        if (result && (first_error == INV_SUCCESS)) {

-            first_error = result;

-        }

-    }

-    return first_error;

-}

-

-/**

- * @}

- */

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+/**
+ *   @defgroup  Start_Manager start_manager
+ *   @brief     Motion Library - Start Manager
+ *              Start Manager
+ *
+ *   @{
+ *       @file start_manager.c
+ *       @brief This handles all the callbacks when inv_start_mpl() is called.
+ */
+
+
+#include <string.h>
+#include "log.h"
+#include "start_manager.h"
+
+typedef inv_error_t (*inv_start_cb_func)();
+struct inv_start_cb_t {
+    int num_cb;
+    inv_start_cb_func start_cb[INV_MAX_START_CB];
+};
+
+static struct inv_start_cb_t inv_start_cb;
+
+/** Initilize the start manager. Typically called by inv_start_mpl();
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_start_manager(void)
+{
+    memset(&inv_start_cb, 0, sizeof(inv_start_cb));
+    return INV_SUCCESS;
+}
+
+/** Removes a callback from start notification
+* @param[in] start_cb function to remove from start notification
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
+{
+    int kk;
+
+    for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
+        if (inv_start_cb.start_cb[kk] == start_cb) {
+            // Found the match
+            if (kk != (inv_start_cb.num_cb-1)) {
+                memmove(&inv_start_cb.start_cb[kk],
+                    &inv_start_cb.start_cb[kk+1],
+                    (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
+            }
+            inv_start_cb.num_cb--;
+            return INV_SUCCESS;
+        }
+    }
+    return INV_ERROR_INVALID_PARAMETER;
+}
+
+/** Register a callback to receive when inv_start_mpl() is called.
+* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
+*            called.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
+{
+    if (inv_start_cb.num_cb >= INV_MAX_START_CB)
+        return INV_ERROR_INVALID_PARAMETER;
+
+    inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
+    inv_start_cb.num_cb++;
+    return INV_SUCCESS;
+}
+
+/** Callback all the functions that want to be notified when inv_start_mpl() was
+* called.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_mpl_start_notification(void)
+{
+    inv_error_t result,first_error;
+    int kk;
+
+    first_error = INV_SUCCESS;
+
+    for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
+        result = inv_start_cb.start_cb[kk]();
+        if (result && (first_error == INV_SUCCESS)) {
+            first_error = result;
+        }
+    }
+    return first_error;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c
index 4b92bfc..3e4e619 100644
--- a/libsensors_iio/software/core/mllite/storage_manager.c
+++ b/libsensors_iio/software/core/mllite/storage_manager.c
@@ -4,6 +4,7 @@
     See included License.txt for License information.
  $
  */
+
 /**
  *   @defgroup  Storage_Manager storage_manager
  *   @brief     Motion Library - Stores Data for functions.
@@ -13,6 +14,9 @@
  *       @file storage_manager.c
  *       @brief Load and Store Manager.
  */
+
+#include <string.h>
+
 #include "storage_manager.h"
 #include "log.h"
 #include "ml_math_func.h"
diff --git a/libsensors_iio/software/core/mpl/accel_auto_cal.h b/libsensors_iio/software/core/mpl/accel_auto_cal.h
index 5a53213..6641a7f 100644
--- a/libsensors_iio/software/core/mpl/accel_auto_cal.h
+++ b/libsensors_iio/software/core/mpl/accel_auto_cal.h
@@ -1,38 +1,38 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-

-/*******************************************************************************

- *

- * $Id$

- *

- ******************************************************************************/

-

-#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__

-#define MLDMP_ACCEL_AUTO_CALIBRATION_H__

-

-#include "mltypes.h"

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-

-inv_error_t inv_enable_in_use_auto_calibration(void);

-inv_error_t inv_disable_in_use_auto_calibration(void);

-inv_error_t inv_stop_in_use_auto_calibration(void);

-inv_error_t inv_start_in_use_auto_calibration(void);

-inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);

-inv_error_t inv_init_in_use_auto_calibration(void);

-void inv_init_accel_maxmin(void);

-void inv_record_good_accel_maxmin(void);

-int inv_get_accel_bias_stage();

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__

-

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__
+#define MLDMP_ACCEL_AUTO_CALIBRATION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_in_use_auto_calibration(void);
+inv_error_t inv_disable_in_use_auto_calibration(void);
+inv_error_t inv_stop_in_use_auto_calibration(void);
+inv_error_t inv_start_in_use_auto_calibration(void);
+inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
+inv_error_t inv_init_in_use_auto_calibration(void);
+void inv_init_accel_maxmin(void);
+void inv_record_good_accel_maxmin(void);
+int inv_get_accel_bias_stage();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__
+
diff --git a/libsensors_iio/software/core/mpl/adv_func.h b/libsensors_iio/software/core/mpl/adv_func.h
deleted file mode 100644
index 3f8595f..0000000
--- a/libsensors_iio/software/core/mpl/adv_func.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
-    See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_ADVFUNC_H__
-#define MLDMP_ADVFUNC_H__
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    float inv_compass_angle(const long *compass, const long *grav, const float *quat);
-    inv_error_t inv_set_dmp_quaternion(long *q);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_ADVFUNC_H__
diff --git a/libsensors_iio/software/core/mpl/auto_calibration.h b/libsensors_iio/software/core/mpl/auto_calibration.h
deleted file mode 100644
index 3dd9827..0000000
--- a/libsensors_iio/software/core/mpl/auto_calibration.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_IN_USE_AUTO_CALIBRATION_H__
-#define MLDMP_IN_USE_AUTO_CALIBRATION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_in_use_auto_calibration(void);
-inv_error_t inv_disable_in_use_auto_calibration(void);
-inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
-inv_error_t inv_init_in_use_auto_calibration(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_IN_USE_AUTO_CALIBRATION_H__
-
diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
index 116b618..5e74a60 100644
--- a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
+++ b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/libsensors_iio/software/core/mpl/build/android/shared.mk b/libsensors_iio/software/core/mpl/build/android/shared.mk
index 79cf9c1..2e15205 100644
--- a/libsensors_iio/software/core/mpl/build/android/shared.mk
+++ b/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -16,6 +16,7 @@ MPL_DIR  = $(INV_ROOT)/software/core/mpl
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -43,11 +44,6 @@ LLINK += -ldl
 LFLAGS += $(CMDLINE_LFLAGS)
 LFLAGS += -shared 
 LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -nostdlib 
-LFLAGS += -fpic 
-LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc 
-LFLAGS += -Wl,--gc-sections 
-LFLAGS += -Wl,--no-whole-archive 
 LFLAGS += -Wl,-shared,-Bsymbolic 
 LFLAGS += $(ANDROID_LINK)
 LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
diff --git a/libsensors_iio/software/core/mpl/build/android/static.mk b/libsensors_iio/software/core/mpl/build/android/static.mk
deleted file mode 100644
index 6ad45de..0000000
--- a/libsensors_iio/software/core/mpl/build/android/static.mk
+++ /dev/null
@@ -1,110 +0,0 @@
-MLLITE_LIB_NAME = mllite
-LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT)
-
-MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
-
-OBJFOLDER = $(CURDIR)/obj
-
-CROSS = arm-none-linux-gnueabi-
-COMP= $(CROSS)gcc
-LINK= $(CROSS)ar cr
-
-MLLITE_DIR = $(MLSDK_ROOT)/mllite
-MPL_DIR = $(MLSDK_ROOT)/mldmp
-MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
-
-include $(MLSDK_ROOT)/Android-common.mk
-
-CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += -Wall -fpic
-CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0
-CFLAGS += -DNDEBUG
-CFLAGS += -D_REENTRANT -DLINUX -DANDROID
-CFLAGS += -I$(MLLITE_DIR) 
-CFLAGS += -I$(MLPLATFORM_DIR)/include
-CFLAGS += -I$(MLSDK_ROOT)/mlutils 
-CFLAGS += -I$(MLSDK_ROOT)/mlapps/common
-CFLAGS += $(MLSDK_INCLUDES)
-CFLAGS += $(MLSDK_DEFINES)
-
-VPATH += $(MLLITE_DIR) 
-VPATH += $(MLSDK_ROOT)/mlutils
-VPATH += $(MLLITE_DIR)/accel 
-VPATH += $(MLLITE_DIR)/compass 
-VPATH += $(MLLITE_DIR)/pressure 
-VPATH += $(MLLITE_DIR)/mlapps/common
-
-####################################################################################################
-## sources
-
-ML_LIBS  = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT)
-
-ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c
-ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c
-ML_SOURCES += $(MLLITE_DIR)/accel.c
-ML_SOURCES += $(MLLITE_DIR)/compass.c
-ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c
-ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c
-ML_SOURCES += $(MLLITE_DIR)/key0_96.c
-ML_SOURCES += $(MLLITE_DIR)/pressure.c
-ML_SOURCES += $(MLLITE_DIR)/ml.c
-ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c
-ML_SOURCES += $(MLLITE_DIR)/ml_init.c
-ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c
-ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c
-ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c
-ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c
-ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c
-ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c
-ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c
-ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c
-ML_SOURCES += $(MLLITE_DIR)/mldl.c
-ML_SOURCES += $(MLLITE_DIR)/mldmp.c
-ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c
-ML_SOURCES += $(MLLITE_DIR)/mlstates.c
-ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c
-ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c
-ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c
-ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c
-ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c
-ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c
-ifeq ($(MPU_NAME),MPU3050)
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c
-else
-ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c
-endif
-
-ML_OBJS := $(addsuffix .o,$(ML_SOURCES))
-ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES))))
-
-####################################################################################################
-## rules
-
-.PHONY: all clean cleanall
-
-all: $(LIBRARY) $(MK_NAME)
-
-$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME)
-	@$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n")
-	$(LINK) $(LIBRARY) $(ML_OBJS_DST)
-	$(CROSS)ranlib $(LIBRARY)
-
-$(OBJFOLDER) : 
-	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
-	mkdir obj
-
-$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
-	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
-	$(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $<
-
-clean : 
-	rm -fR $(OBJFOLDER)
-
-cleanall : 
-	rm -fR $(LIBRARY) $(OBJFOLDER)
-
diff --git a/libsensors_iio/software/core/mpl/fast_no_motion.h b/libsensors_iio/software/core/mpl/fast_no_motion.h
index 2a33093..24bc0d5 100644
--- a/libsensors_iio/software/core/mpl/fast_no_motion.h
+++ b/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -28,12 +28,14 @@ extern "C" {
     void inv_set_default_number_of_samples(int N);
     inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);
     inv_error_t inv_update_fast_nomot(long *gyro);
-    
-    void inv_get_fast_nomot_accel_param(long *cntr, float *param);
-    void inv_get_fast_nomot_compass_param(long *cntr, float *param);
-    void inv_set_fast_nomot_accel_threshold(float thresh);
-    void inv_set_fast_nomot_compass_threshold(float thresh);
-    void int_set_fast_nomot_gyro_threshold(float thresh);
+

+    void inv_get_fast_nomot_accel_param(long *cntr, long long *param);

+    void inv_get_fast_nomot_compass_param(long *cntr, long long *param);

+    void inv_set_fast_nomot_accel_threshold(long long thresh);

+    void inv_set_fast_nomot_compass_threshold(long long thresh);

+    void int_set_fast_nomot_gyro_threshold(long long thresh);

+

+    void inv_fnm_debug_print(void);

 
 #ifdef __cplusplus
 }
diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/libsensors_iio/software/core/mpl/fusion_9axis.h
index 616694a..1ba1ebb 100644
--- a/libsensors_iio/software/core/mpl/fusion_9axis.h
+++ b/libsensors_iio/software/core/mpl/fusion_9axis.h
@@ -26,6 +26,8 @@ extern "C" {
     inv_error_t inv_disable_9x_sensor_fusion(void);
     inv_error_t inv_start_9x_sensor_fusion(void);
     inv_error_t inv_stop_9x_sensor_fusion(void);
+    inv_error_t inv_9x_fusion_set_mag_fb(double fb);

+    inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);

 
 #ifdef __cplusplus
 }
diff --git a/libsensors_iio/software/core/mpl/interpolator.h b/libsensors_iio/software/core/mpl/interpolator.h
deleted file mode 100644
index 5eb571d..0000000
--- a/libsensors_iio/software/core/mpl/interpolator.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-#ifndef INTERPOLATOR_H
-#define INTERPOLATOR_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-//#include "mltypes.h"
-
-    /* ------------ */
-    /* - Defines. - */
-    /* ------------ */
-
-#define MAX_INTERPOLATION (20)
-
-typedef struct  {
-    long x[2];
-    long y[4];
-} tInterpolate2;
-typedef struct  {
-    long x[2];
-    long y[6];
-} tInterpolate3;
-typedef struct  {
-    long x[5];
-    long y[10];
-    int idx1;
-} tInterpolate5;
-typedef struct {
-    tInterpolate2 state1;
-    tInterpolate2 state2;
-} tInterpolate4;
-typedef struct {
-    tInterpolate3 state1;
-    tInterpolate2 state2;
-} tInterpolate6;
-typedef struct {
-    tInterpolate2 state1;
-    tInterpolate4 state2;
-} tInterpolate8;
-typedef struct {
-    tInterpolate3 state1;
-    tInterpolate3 state2;
-} tInterpolate9;
-typedef struct {
-    tInterpolate5 state1;
-    tInterpolate2 state2;
-} tInterpolate10;
-typedef struct {
-    tInterpolate4 state1;
-    tInterpolate3 state2;
-} tInterpolate12;
-typedef struct {
-    tInterpolate5 state1;
-    tInterpolate3 state2;
-} tInterpolate15;
-typedef struct {
-    tInterpolate2 state1;
-    tInterpolate8 state2;
-} tInterpolate16;
-typedef struct {
-    tInterpolate9 state1;
-    tInterpolate2 state2;
-} tInterpolate18;
-typedef struct {
-    tInterpolate10 state1;
-    tInterpolate2 state2;
-} tInterpolate20;
-
-typedef  union {
-    tInterpolate2 u2;
-    tInterpolate3 u3;
-    tInterpolate4 u4;
-    tInterpolate5 u5;
-    tInterpolate6 u6;
-    tInterpolate8 u8;
-    tInterpolate9 u9;
-    tInterpolate10 u10;
-    tInterpolate12 u12;
-    tInterpolate15 u15;
-    tInterpolate16 u16;
-    tInterpolate18 u18;
-    tInterpolate20 u20;
-} tInterpolateState;
-
-    /* --------------------- */
-    /* - Function p-types. - */
-    /* --------------------- */
-int inv_get_interp_amount( int x );
-int inv_interpolate( int amount, long input, long *output, tInterpolateState *state );
-long inv_fxmult( long x, long y );
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* INTERPOLATOR_H */
diff --git a/libsensors_iio/software/core/mpl/inv_log.h b/libsensors_iio/software/core/mpl/inv_log.h
deleted file mode 100644
index 972844b..0000000
--- a/libsensors_iio/software/core/mpl/inv_log.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#include "mltypes.h"
-#ifndef INV_INV_LOG_H__
-#define INV_INV_LOG_H__
-
-#define INV_LOGE(s)
-
-#endif // INV_INV_LOG_H__
diff --git a/libsensors_iio/software/core/mpl/inv_math.h b/libsensors_iio/software/core/mpl/inv_math.h
index 6620bbf..175511a 100644
--- a/libsensors_iio/software/core/mpl/inv_math.h
+++ b/libsensors_iio/software/core/mpl/inv_math.h
@@ -1,8 +1,8 @@
-/* math.h has many functions and defines that are not consistent across 

-* platforms. This address that */

-

-#ifdef _WINDOWS

-#define _USE_MATH_DEFINES

-#endif

-

-#include <math.h>

+/* math.h has many functions and defines that are not consistent across 
+* platforms. This address that */
+
+#ifdef _WINDOWS
+#define _USE_MATH_DEFINES
+#endif
+
+#include <math.h>
diff --git a/libsensors_iio/software/core/mpl/invensense_adv.h b/libsensors_iio/software/core/mpl/invensense_adv.h
index 9e59c18..12932c9 100644
--- a/libsensors_iio/software/core/mpl/invensense_adv.h
+++ b/libsensors_iio/software/core/mpl/invensense_adv.h
@@ -28,3 +28,4 @@
 #include "quaternion_supervisor.h"
 #include "mag_disturb.h"
 #include "quat_accuracy_monitor.h"
+#include "shake.h"
diff --git a/libsensors_iio/software/core/mpl/mlsetinterrupts.h b/libsensors_iio/software/core/mpl/mlsetinterrupts.h
deleted file mode 100644
index a81dabb..0000000
--- a/libsensors_iio/software/core/mpl/mlsetinterrupts.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- $License:
-   Copyright (c) 2008 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-#ifndef MLSETINTERRUPT_H
-#define MLSETINTERRUPT_H
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    /* DEPRECATED - Scheduled for removal. Do not use */
-    inv_error_t MLSetInterrupts(unsigned short interrupts);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* MLSETINTERRUPT_H */
diff --git a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h
deleted file mode 100644
index 3779381..0000000
--- a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/******************************************************************************
- *
- * $Id: mlsupervisor_9axis.h 6123 2011-09-30 18:21:11Z mcaramello $
- *
- *****************************************************************************/
-
-#ifndef MLDMP_MLSUPERVISOR_H__
-#define MLDMP_MLSUPERVISOR_H__
-
-#include "mltypes.h"
-
-//#include "temp_comp.h"
-
-struct inv_fusion_t {
-    int compassCount;
-    long quat[4];
-};
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_9x_fusion(void);
-inv_error_t inv_disable_9x_fusion(void);
-
-inv_error_t inv_enable_9x_fusion_legacy(void);
-inv_error_t inv_disable_9x_fusion_legacy(void);
-
-inv_error_t inv_enable_9x_fusion_new(void);
-inv_error_t inv_disable_9x_fusion_new(void);
-
-inv_error_t inv_enable_9x_fusion_basic(void);
-inv_error_t inv_disable_9x_fusion_basic(void);
-
-inv_error_t inv_enable_9x_fusion_external(void);
-inv_error_t inv_disable_9x_fusion_external(void);
-
-inv_error_t inv_enable_maintain_heading(void);
-inv_error_t inv_disable_maintain_heading(void);
-
-void inv_set_compass_state(long compassState, long accState, 
-                           unsigned long deltaTime,
-                           int magDisturb, int gotBias,
-                           int *new_state,
-                           int *new_accuracy);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // MLDMP_MLSUPERVISOR_H__
diff --git a/libsensors_iio/software/core/mpl/motion_no_motion.h b/libsensors_iio/software/core/mpl/motion_no_motion.h
index 01cf1c0..188c78b 100644
--- a/libsensors_iio/software/core/mpl/motion_no_motion.h
+++ b/libsensors_iio/software/core/mpl/motion_no_motion.h
@@ -1,28 +1,28 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
  */
-#ifndef INV_MOTION_NO_MOTION_H__

-#define INV_MOTION_NO_MOTION_H__

-

-#include "mltypes.h"

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-

-inv_error_t inv_enable_motion_no_motion(void);

-inv_error_t inv_disable_motion_no_motion(void);

-inv_error_t inv_init_motion_no_motion(void);

-inv_error_t inv_start_motion_no_motion(void);

-inv_error_t inv_stop_motion_no_motion(void);

-

-inv_error_t inv_set_no_motion_time(long time_ms);

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif // INV_MOTION_NO_MOTION_H__

+#ifndef INV_MOTION_NO_MOTION_H__
+#define INV_MOTION_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_motion_no_motion(void);
+inv_error_t inv_disable_motion_no_motion(void);
+inv_error_t inv_init_motion_no_motion(void);
+inv_error_t inv_start_motion_no_motion(void);
+inv_error_t inv_stop_motion_no_motion(void);
+
+inv_error_t inv_set_no_motion_time(long time_ms);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MOTION_NO_MOTION_H__
diff --git a/libsensors_iio/software/core/mpl/orientation.h b/libsensors_iio/software/core/mpl/orientation.h
deleted file mode 100644
index ab4e45e..0000000
--- a/libsensors_iio/software/core/mpl/orientation.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-#ifndef MLDMP_ORIENTATION_H__
-#define MLDMP_ORIENTATION_H__
-
-#include "mltypes.h"
-/*******************************************************************************/
-/*    Orientations                                                             */
-/*******************************************************************************/
-
-#define INV_X_UP                          0x01
-#define INV_X_DOWN                        0x02
-#define INV_Y_UP                          0x04
-#define INV_Y_DOWN                        0x08
-#define INV_Z_UP                          0x10
-#define INV_Z_DOWN                        0x20
-#define INV_ORIENTATION_ALL               0x3F
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    inv_error_t inv_enable_orientation(void);
-    inv_error_t inv_disable_orientation(void);
-    inv_error_t inv_set_orientation(int orientation);
-    inv_error_t inv_set_orientation_cb(void (*callback)(unsigned short));
-    inv_error_t inv_get_orientation(int *orientation);
-    inv_error_t inv_get_orientation_state(int * state);
-    inv_error_t inv_set_orientation_interrupt(unsigned char on);
-    inv_error_t inv_set_orientation_thresh(float angle,
-                                       float hysteresis,
-                                       unsigned long time, 
-                                       unsigned int axis);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // MLDMP_ORIENTATION_H__
diff --git a/libsensors_iio/software/core/mpl/progressive_no_motion.h b/libsensors_iio/software/core/mpl/progressive_no_motion.h
deleted file mode 100644
index 99333e3..0000000
--- a/libsensors_iio/software/core/mpl/progressive_no_motion.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/******************************************************************************
- *
- * $Id:$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_PROG_NO_MOTION_H__
-#define MLDMP_PROG_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#define PROG_NO_MOTION  1
-#define PROG_MOTION     2
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* APIs */
-inv_error_t inv_enable_prog_no_motion(void);
-inv_error_t inv_disable_prog_no_motion(void);
-
-/* internal use */
-int inv_get_prog_no_motion_enabled(void);
-void inv_get_prog_no_motion_bias_changed(void);
-int inv_get_prog_no_motion_state(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_PROG_NO_MOTION_H__
diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
index 2cf7a50..5ee0573 100644
--- a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
+++ b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
@@ -1,70 +1,71 @@
-/*

-	quat_accuracy_monitor.h

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-

-/*******************************************************************************

- *

- * $Id:$

- *

- ******************************************************************************/

-

-#ifndef QUAT_ACCURARCY_MONITOR_H__

-#define QUAT_ACCURARCY_MONITOR_H__

-

-#include "mltypes.h"

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-enum accuracy_signal_type_e {

-    TYPE_NAV_QUAT,

-    TYPE_GAM_QUAT,

-    TYPE_NAV_QUAT_ADVANCED,

-    TYPE_GAM_QUAT_ADVANCED,

-    TYPE_NAV_QUAT_BASIC,

-    TYPE_GAM_QUAT_BASIC,

-    TYPE_MAG,

-    TYPE_GYRO,

-    TYPE_ACCEL,

-};

-

-inv_error_t inv_init_quat_accuracy_monitor(void);

-

-void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);

-double get_accuracy_threshold(enum accuracy_signal_type_e type);

-void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);

-int get_accuracy_weight(enum accuracy_signal_type_e type);

-

-int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);

-

-void inv_reset_quat_accuracy(void);

-double get_6axis_correction_term(void);

-double get_9axis_correction_term(void);

-int get_9axis_accuracy_state();

-

-void set_6axis_error_average(double value);

-double get_6axis_error_bound(void);

-double get_compass_correction(void);

-double get_9axis_error_bound(void);

-

-float get_confidence_interval(void);

-void set_compass_uncertainty(float value);

-

-inv_error_t inv_enable_quat_accuracy_monitor(void);

-inv_error_t inv_disable_quat_accuracy_monitor(void);

-inv_error_t inv_start_quat_accuracy_monitor(void);

-inv_error_t inv_stop_quat_accuracy_monitor(void);

-

-double get_compassNgravity(void);

-double get_init_compassNgravity(void);

-

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif // QUAT_ACCURARCY_MONITOR_H__

+/*
+	quat_accuracy_monitor.h
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef QUAT_ACCURARCY_MONITOR_H__
+#define QUAT_ACCURARCY_MONITOR_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+enum accuracy_signal_type_e {
+    TYPE_NAV_QUAT,
+    TYPE_GAM_QUAT,
+    TYPE_NAV_QUAT_ADVANCED,
+    TYPE_GAM_QUAT_ADVANCED,
+    TYPE_NAV_QUAT_BASIC,
+    TYPE_GAM_QUAT_BASIC,
+    TYPE_MAG,
+    TYPE_GYRO,
+    TYPE_ACCEL,
+};
+
+inv_error_t inv_init_quat_accuracy_monitor(void);
+
+void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
+double get_accuracy_threshold(enum accuracy_signal_type_e type);
+void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
+int get_accuracy_weight(enum accuracy_signal_type_e type);
+
+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
+
+void inv_reset_quat_accuracy(void);
+double get_6axis_correction_term(void);
+double get_9axis_correction_term(void);
+int get_9axis_accuracy_state();
+
+void set_6axis_error_average(double value);
+double get_6axis_error_bound(void);
+double get_compass_correction(void);
+double get_9axis_error_bound(void);
+
+float get_confidence_interval(void);
+void set_compass_uncertainty(float value);
+
+inv_error_t inv_enable_quat_accuracy_monitor(void);
+inv_error_t inv_disable_quat_accuracy_monitor(void);
+inv_error_t inv_start_quat_accuracy_monitor(void);
+inv_error_t inv_stop_quat_accuracy_monitor(void);
+
+double get_compassNgravity(void);
+double get_init_compassNgravity(void);
+
+float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
index 532e8af..722c0d9 100644
--- a/libsensors_iio/software/core/mpl/quaternion_supervisor.h
+++ b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -1,26 +1,27 @@
-/*

- $License:

-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.

-    See included License.txt for License information.

- $

- */

-#ifndef INV_QUATERNION_SUPERVISOR_H__

-#define INV_QUATERNION_SUPERVISOR_H__

-

-#include "mltypes.h"

-

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-

-inv_error_t inv_enable_quaternion(void);

-inv_error_t inv_disable_quaternion(void);

-inv_error_t inv_init_quaternion(void);

-inv_error_t inv_start_quaternion(void);

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif // INV_QUATERNION_SUPERVISOR_H__

+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_QUATERNION_SUPERVISOR_H__
+#define INV_QUATERNION_SUPERVISOR_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_quaternion(void);
+inv_error_t inv_disable_quaternion(void);
+inv_error_t inv_init_quaternion(void);
+inv_error_t inv_start_quaternion(void);
+void inv_set_quaternion(long *quat);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/libsensors_iio/software/core/mpl/sensor_moments.h b/libsensors_iio/software/core/mpl/sensor_moments.h
deleted file mode 100644
index 73eb363..0000000
--- a/libsensors_iio/software/core/mpl/sensor_moments.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
-    See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_SENSOR_MOMENTS_H__
-#define MLDMP_SENSOR_MOMENTS_H__
-
-#include "mltypes.h"
-
-enum moment_ord {
-	SECOND_ORD=0,
-	THIRD_ORD,
-	FOURTH_ORD,
-	MAX_ORD
-};
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-	inv_error_t inv_enable_sm(void);

-	inv_error_t inv_disable_sm(void);

-	inv_error_t inv_sm_record_data(float sample, void *sensor);

-	inv_error_t inv_sm_update_evt_act_state(int motion);

-	void *inv_init_sm(enum moment_ord);

-	float inv_sm_get_filtered_data(void *sensor);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // MLDMP_SENSOR_MOMENTS_H__
-
diff --git a/libsensors_iio/software/core/mpl/shake.h b/libsensors_iio/software/core/mpl/shake.h
new file mode 100644
index 0000000..8775a4c
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/shake.h
@@ -0,0 +1,94 @@
+/*
+ $License:
+    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+    See included License.txt for License information.
+ $
+ */
+#ifndef INV_SHAKE_H__
+#define INV_SHAKE_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/* ------------ */
+    /* - Defines. - */
+    /* ------------ */
+
+    #define STATE_ZERO                             0
+    #define STATE_INIT_1                           1
+    #define STATE_INIT_2                           2
+    #define STATE_DETECT                           3
+
+	struct t_shake_config_params {
+		long shake_time_min_ms;
+		long shake_time_max_ms;
+		long shake_time_min;
+        long shake_time_max;
+		unsigned char shake_time_set;
+		long shake_time_saved;
+        float shake_deriv_thr;
+        int zero_cross_thr;
+        float accel_delta_min;
+        float accel_delta_max;
+		unsigned char interp_enable;
+	};
+
+	struct t_shake_state_params {
+		unsigned char state;
+        float accel_peak_high;
+        float accel_peak_low;
+        float accel_range;
+        int num_zero_cross;
+        short curr_shake_time;
+		int deriv_major_change;
+		int deriv_major_sign;
+        float accel_buffer[200];
+        float delta_buffer[200];
+	};
+
+	struct t_shake_data_params {
+		float accel_prev;
+		float accel_curr;
+		float delta_prev;
+		float delta_curr;
+		float delta_prev_buffer;
+	};
+
+	struct t_shake_results {
+		//unsigned char shake_int;
+		int shake_number;
+	};
+
+	struct t_shake_cb {
+       void (*shake_callback)(struct t_shake_results *shake_results);            
+    };
+
+
+    /* --------------------- */
+    /* - Function p-types. - */
+    /* --------------------- */
+	inv_error_t inv_enable_shake(void);
+    inv_error_t inv_disable_shake(void);
+    inv_error_t inv_init_shake(void);
+    inv_error_t inv_start_shake(void);
+	int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
+	void inv_config_shake_time_params(long sample_time_ms);
+	void inv_set_shake_accel_delta_min(float accel_g);
+	void inv_set_shake_accel_delta_max(float accel_g);
+	void inv_set_shake_zero_cross_thresh(int num_zero_cross);
+	void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
+	void inv_set_shake_time_min_ms(long time_ms);
+	void inv_set_shake_time_max_ms(long time_ms);
+	void inv_enable_shake_data_interpolation(unsigned char en);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_SHAKE__
\ No newline at end of file
diff --git a/libsensors_iio/software/core/mpl/state_storage.h b/libsensors_iio/software/core/mpl/state_storage.h
deleted file mode 100644
index c1eb47b..0000000
--- a/libsensors_iio/software/core/mpl/state_storage.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- $License:
-    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
-    See included License.txt for License information.
- $
- */
-#ifndef INV_STATE_STORAGE_H__
-#define INV_STATE_STORAGE_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_store_data(const void *data, size_t size, unsigned long module,
-                           unsigned long version);
-inv_error_t inv_load_data(void *data, size_t size, unsigned long module,
-                          unsigned long version);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_STATE_STORAGE_H__
diff --git a/libsensors_iio/software/simple_apps/common/external_hardware.h b/libsensors_iio/software/simple_apps/common/external_hardware.h
deleted file mode 100644
index 55e3b20..0000000
--- a/libsensors_iio/software/simple_apps/common/external_hardware.h
+++ /dev/null
@@ -1,156 +0,0 @@
-/*
-    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
deleted file mode 100644
index 2936109..0000000
--- a/libsensors_iio/software/simple_apps/common/fopenCMake.c
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- $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
deleted file mode 100644
index c5eba39..0000000
--- a/libsensors_iio/software/simple_apps/common/fopenCMake.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- $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
deleted file mode 100644
index 2a9487c..0000000
--- a/libsensors_iio/software/simple_apps/common/gestureMenu.c
+++ /dev/null
@@ -1,725 +0,0 @@
-/*
- $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
deleted file mode 100644
index 8f804e1..0000000
--- a/libsensors_iio/software/simple_apps/common/gestureMenu.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- $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
deleted file mode 100644
index 4d634bd..0000000
--- a/libsensors_iio/software/simple_apps/common/helper.c
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- $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
deleted file mode 100644
index b2da520..0000000
--- a/libsensors_iio/software/simple_apps/common/helper.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- $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
deleted file mode 100644
index 25b0df6..0000000
--- a/libsensors_iio/software/simple_apps/common/mlerrorcode.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- $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
deleted file mode 100644
index 9a35792..0000000
--- a/libsensors_iio/software/simple_apps/common/mlerrorcode.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*
- $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
deleted file mode 100644
index f11bce9..0000000
--- a/libsensors_iio/software/simple_apps/common/mlsetup.c
+++ /dev/null
@@ -1,1722 +0,0 @@
-/*
- $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
deleted file mode 100644
index 06fa9f4..0000000
--- a/libsensors_iio/software/simple_apps/common/mlsetup.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- $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
deleted file mode 100644
index 7b40a8c..0000000
--- a/libsensors_iio/software/simple_apps/common/slave.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
- $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
deleted file mode 100644
index 228abf7..0000000
--- a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
+++ /dev/null
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
deleted file mode 100644
index b01fdfb..0000000
--- a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
+++ /dev/null
@@ -1,23 +0,0 @@
-#### 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
deleted file mode 100644
index e15b20d..0000000
--- a/libsensors_iio/software/simple_apps/console/linux/console_test.c
+++ /dev/null
@@ -1,742 +0,0 @@
-/*******************************************************************************
- * $Id: $
- ******************************************************************************/
- 
-/*******************************************************************************
- *
- * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved.
- *
- ******************************************************************************/
-//#include <linux/conio.h>
-//#include <fcntl.h>
-//#include <termios.h>
-//#include <unistd.h>
-
-//#if 0
-#include <stdio.h>
-#include <time.h>
-#include <sys/select.h>
-#include <unistd.h>
-#include <string.h>
-#include <sys/ioctl.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <stdlib.h>
-#include <linux/input.h>
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "console_test"
-
-#include "mlos.h"
-#include "invensense.h"
-#include "invensense_adv.h"
-#include "inv_sysfs_utils.h"
-#include "ml_stored_data.h"
-#include "ml_math_func.h"
-#include "ml_load_dmp.h"
-/*#else
-#include <unistd.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <sys/stat.h>
-#include <stdlib.h>
-#include <features.h>
-#include <dirent.h>
-#include <string.h>
-#include <poll.h>
-#include <stddef.h>
-#include <linux/input.h>
-#include <time.h>
-#include <linux/time.h>
-#include "inv_sysfs_utils.h"
-#include "invensense.h"
-#include "invensense_adv.h"
-#include "ml_stored_data.h"
-#include "ml_math_func.h"
-#include "ml_load_dmp.h"
-#include "log.h"
-#endif*/
-
-/* TODO: add devices as needed. */
-#define ITG3500         (0)
-#define MPU6050         (1)
-#define BMA250          (2)
-#define NUM_DEVICES     (ITG3500 + MPU6050 + BMA250)
-
-#define DEVICE          MPU6050
-#define DMP_IMAGE       dmp_firmware_200_latest
-#define SIX_AXES        6
-#define NINE_AXES       9
-
-#if 0
-struct input_event {
-       struct timeval time;
-       __u16 type;
-       __u16 code;
-       __s32 value;
-};
-#endif
-
-/* TODO: Add paths to other attributes.
- * TODO: Input device paths depend on the module installation order.
- */
-const struct inv_sysfs_names_s filenames[NUM_DEVICES] = {
-    {        /* ITG3500 */
-        .buffer             = "/dev/input/event0",
-        .enable             = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable",
-        .raw_data           = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro",
-        .temperature        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature",
-        .fifo_rate          = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate",
-        .power_state        = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state",
-        .fsr                = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR",
-        .lpf                = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf",
-        .scale              = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale",
-        .temp_scale         = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale",
-        .temp_offset        = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset",
-        .self_test          = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test",
-        .accel_en           = NULL,
-        .accel_fifo_en      = NULL,
-        .accel_fs           = NULL,
-        .clock_source       = NULL,
-        .early_suspend_en   = NULL,
-        .firmware_loaded    = NULL,
-        .gyro_en            = NULL,
-        .gyro_fifo_en       = NULL,
-        .key                = NULL,
-        .raw_accel          = NULL,
-        .reg_dump           = NULL,
-        .tap_on             = NULL,
-        .dmp_firmware       = NULL
-    },
-
-    {        /* MPU6050 */
-        .buffer             = "/dev/input/event0",
-        .enable             = "/sys/class/invensense/mpu/enable",
-        .raw_data           = "/sys/class/invensense/mpu/raw_gyro",
-        .temperature        = "/sys/class/invensense/mpu/temperature",
-        .fifo_rate          = "/sys/class/invensense/mpu/fifo_rate",
-        .power_state        = "/sys/class/invensense/mpu/power_state",
-        .fsr                = "/sys/class/invensense/mpu/FSR",
-        .lpf                = "/sys/class/invensense/mpu/lpf",
-        .scale              = "/sys/class/invensense/mpu/gyro_scale",
-        .temp_scale         = "/sys/class/invensense/mpu/temp_scale",
-        .temp_offset        = "/sys/class/invensense/mpu/temp_offset",
-        .self_test          = "/sys/class/invensense/mpu/self_test",
-        .accel_en           = "/sys/class/invensense/mpu/accl_enable",
-        .accel_fifo_en      = "/sys/class/invensense/mpu/accl_fifo_enable",
-        .accel_fs           = "/sys/class/invensense/mpu/accl_fs",
-        .clock_source       = "/sys/class/invensense/mpu/clock_source",
-        .early_suspend_en   = "/sys/class/invensense/mpu/early_suspend_enable",
-           .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded",
-        .gyro_en            = "/sys/class/invensense/mpu/gyro_enable",
-        .gyro_fifo_en       = "/sys/class/invensense/mpu/gyro_fifo_enable",
-        .key                = "/sys/class/invensense/mpu/key",
-        .raw_accel          = "/sys/class/invensense/mpu/raw_accl",
-        .reg_dump           = "/sys/class/invensense/mpu/reg_dump",
-        .tap_on             = "/sys/class/invensense/mpu/tap_on",
-         .dmp_firmware      = "/sys/class/invensense/mpu/dmp_firmware"
-    },
-
-    {        /* BMA250 */
-        .buffer             = "/dev/input/input/event1",
-        .enable             = "/sys/devices/virtual/input/input1/enable",
-        .raw_data           = "/sys/devices/virtual/input/input1/value",
-        .temperature        = NULL,
-        .fifo_rate          = NULL,
-        .power_state        = NULL,
-        .fsr                = NULL,
-        .lpf                = NULL,
-        .scale              = NULL,
-        .temp_scale         = NULL,
-        .temp_offset        = NULL,
-        .self_test          = NULL,
-        .accel_en           = NULL,
-        .accel_fifo_en      = NULL,
-        .accel_fs           = NULL,
-        .clock_source       = NULL,
-        .early_suspend_en   = NULL,
-        .firmware_loaded    = NULL,
-        .gyro_en            = NULL,
-        .gyro_fifo_en       = NULL,
-        .key                = NULL,
-        .raw_accel          = NULL,
-        .reg_dump           = NULL,
-        .tap_on             = NULL,
-        .dmp_firmware       = NULL
-    }
-};
-
-static void (*s_func_cb) (void);
-
-int inv_read_data(char *names, char *data)
-{
-    char str[8];
-    int count;
-    short s_data;
-
-    count = inv_sysfs_read((char*)names, sizeof(str), str);
-    if (count < 0)
-        return count;
-    count = sscanf(str, "%hd", &s_data);
-    *data = s_data;
-    if (count < 1)
-        return -EAGAIN;
-    return count;
-
-}
-
-void fifoCB(void)
-{
-    if (1) {
-        float gyro[3];
-        float accel[3];
-        float orient[3];
-        float rv[3];
-
-        int8_t accuracy;
-        inv_time_t timestamp;
-
-        printf("/*************************************************\n"); 
-        inv_get_sensor_type_gyroscope(gyro, &accuracy, &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/gesture_test/build/android/inv_gesture_test-shared b/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
new file mode 100644
index 0000000..ede049d
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk b/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
index 7f6cc43..7655e4d 100644
--- a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
+++ b/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
@@ -1,4 +1,4 @@
-EXEC = input_gyro$(SHARED_APP_SUFFIX)
+EXEC = inv_gesture_test$(SHARED_APP_SUFFIX)
 
 MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
 
@@ -11,9 +11,7 @@ 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
 
@@ -49,20 +47,8 @@ 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
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
 
 LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
@@ -87,7 +73,7 @@ all: $(EXEC) $(MK_NAME)
 
 $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
 	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
 
 $(OBJFOLDER) :
 	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
new file mode 100644
index 0000000..75d93cf
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for inv_gesture_test ####
+
+# headers
+#HEADERS += 
+
+# sources
+SOURCES := $(APP_DIR)/inv_gesture_test.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR)
diff --git a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
new file mode 100644
index 0000000..d38d478
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
@@ -0,0 +1,535 @@
+/**
+ *  Gesture Test application for Invensense's MPU6/9xxx (w/ DMP).
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include "invensense.h"
+#include "ml_math_func.h"
+#include "storage_manager.h"
+#include "ml_stored_data.h"
+#include "ml_sysfs_helper.h"
+
+#define DEBUG_PRINT     /* Uncomment to print Gyro & Accel read from Driver */
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
+#define FLICK_UPPER_THRES       3147790
+#define FLICK_LOWER_THRES       -3147790
+#define FLICK_COUNTER           50
+#define POLL_TIME               2000 // 2sec
+
+#define FALSE   0
+#define TRUE    1
+
+char *sysfs_names_ptr;
+
+struct sysfs_attrbs {
+    char *enable;
+    char *power_state;
+    char *dmp_on;
+    char *dmp_int_on;
+    char *self_test;
+    char *dmp_firmware;
+    char *firmware_loaded;
+    char *display_orientation_on;
+    char *orientation_on;
+    char *event_flick;
+    char *event_display_orientation;
+    char *event_orientation;
+    char *event_tap;
+    char *flick_axis;
+    char *flick_counter;
+    char *flick_int_on;
+    char *flick_lower;
+    char *flick_upper;
+    char *flick_message_on;
+    char *tap_min_count;
+    char *tap_on;
+    char *tap_threshold;
+    char *tap_time;
+} mpu;
+
+enum {
+    tap,
+    flick,
+    gOrient,
+    orient,
+    numDMPFeatures
+};
+
+struct pollfd pfd[numDMPFeatures];
+
+/*******************************************************************************
+ *                       DMP Feature Supported Functions
+ ******************************************************************************/
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE *fp;
+
+    fp = fopen(filename, "r");
+    if (fp!=NULL) {
+        fscanf(fp, "%d\n", var);
+	fclose(fp);
+    } else {
+        MPL_LOGE("ERR open file to read");
+        res= -1;
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int data)
+{
+    int res=0;
+    FILE  *fp;
+
+    fp = fopen(filename, "w");
+    if (fp!=NULL) {
+        fprintf(fp, "%d\n", data);
+	fclose(fp);
+    } else {
+        MPL_LOGE("ERR open file to write");
+        res= -1;
+    }
+    return res;
+}
+
+/**************************************************
+    This _kbhit() function is courtesy from Web
+***************************************************/
+int _kbhit() {
+    static const int STDIN = 0;
+    static bool initialized = false;
+
+    if (! initialized) {
+        // Use termios to turn off line buffering
+        struct termios term;
+        tcgetattr(STDIN, &term);
+        term.c_lflag &= ~ICANON;
+        tcsetattr(STDIN, TCSANOW, &term);
+        setbuf(stdin, NULL);
+        initialized = true;
+    }
+
+    int bytesWaiting;
+    ioctl(STDIN, FIONREAD, &bytesWaiting);
+    return bytesWaiting;
+}
+
+int inv_init_sysfs_attributes(void)
+{
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    sysfs_names_ptr =
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        MPL_LOGE("couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
+    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+    sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
+    sprintf(mpu.event_flick, "%s%s", sysfs_path, "/event_flick");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+    sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
+    sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
+    sprintf(mpu.flick_axis, "%s%s", sysfs_path, "/flick_axis");
+    sprintf(mpu.flick_counter, "%s%s", sysfs_path, "/flick_counter");
+    sprintf(mpu.flick_int_on, "%s%s", sysfs_path, "/flick_int_on");
+    sprintf(mpu.flick_lower, "%s%s", sysfs_path, "/flick_lower");
+    sprintf(mpu.flick_upper, "%s%s", sysfs_path, "/flick_upper");
+    sprintf(mpu.flick_message_on, "%s%s", sysfs_path, "/flick_message_on");
+    sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+    sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
+    sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        MPL_LOGE("sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+int DmpFWloaded()
+{
+    int res;
+    read_sysfs_int(mpu.firmware_loaded, &res);
+    return res;
+}
+
+int enable_flick(int en)
+{
+   int res=0;
+   int flickUpper=0, flickLower=0, flickCounter=0;
+
+   if (write_sysfs_int(mpu.flick_int_on, en) < 0) {
+       printf("GT:ERR-can't write 'flick_int_on'");
+       res= -1;
+   }
+
+   if (en) {
+       flickUpper= FLICK_UPPER_THRES;
+       flickLower= FLICK_LOWER_THRES;
+       flickCounter= FLICK_COUNTER;
+   }
+
+   if (write_sysfs_int(mpu.flick_upper, flickUpper) < 0) {
+       printf("GT:ERR-can't write 'flick_upper'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_lower, flickLower) < 0) {
+       printf("GT:ERR-can't write 'flick_lower'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_counter, flickCounter) < 0) {
+       printf("GT:ERR-can't write 'flick_counter'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_message_on, 0) < 0) {
+       printf("GT:ERR-can't write 'flick_message_on'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_axis, 0) < 0) {
+       printf("GT:ERR_can't write 'flick_axis'");
+       res= -1;
+   }
+
+   return res;
+}
+
+int enable_tap(int en)
+{
+    if (write_sysfs_int(mpu.tap_on, en) < 0) {
+        printf("GT:ERR-can't write 'tap_on'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int enable_displ_orient(int en)
+{
+    if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'display_orientation_en'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int enable_orient(int en)
+{
+    if (write_sysfs_int(mpu.orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'orientation_on'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int flickHandler()
+{
+    FILE *fp;
+    int data;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Flick Handler\n");
+#endif
+
+    fp = fopen(mpu.event_flick, "rt");
+    fscanf(fp, "%d\n", &data);
+    fclose (fp);
+
+    printf("Flick= %x\n", data);
+
+    return 0;
+}
+
+int tapHandler()
+{
+    FILE *fp;
+    int tap, tap_dir, tap_num;
+
+    fp = fopen(mpu.event_tap, "rt");
+    fscanf(fp, "%d\n", &tap);
+    fclose(fp);
+
+    tap_dir = tap/8;
+    tap_num = tap%8 + 1;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Tap Handler **\n");
+    printf("Tap= %x\n", tap);
+    printf("Tap Dir= %x\n", tap_dir);
+    printf("Tap Num= %x\n", tap_num);
+#endif
+
+    switch (tap_dir) {
+        case 1:
+            printf("Tap Axis->X Pos\n");
+            break;
+        case 2:
+            printf("Tap Axis->X Neg\n");
+            break;
+        case 3:
+            printf("Tap Axis->Y Pos\n");
+            break;
+        case 4:
+            printf("Tap Axis->Y Neg\n");
+            break;
+        case 5:
+            printf("Tap Axis->Z Pos\n");
+            break;
+        case 6:
+            printf("Tap Axis->Z Neg\n");
+            break;
+        default:
+            printf("Tap Axis->Unknown\n");
+            break;
+    }
+
+    return 0;
+}
+
+int googleOrientHandler()
+{
+    FILE *fp;
+    int orient;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Google Orient Handler\n");
+#endif
+
+    fp = fopen(mpu.event_display_orientation, "rt");
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+    printf("Google Orient-> %d\n", orient);
+
+    return 0;
+}
+
+int orientHandler()
+{
+    FILE *fp;
+    int orient;
+
+    fp = fopen(mpu.event_orientation, "rt");
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+#ifdef DEBUG_PRINT
+    printf("GT:Reg Orient Handler\n");
+#endif
+
+    if (orient & 0x01)
+	printf("Orient->X Up\n");
+
+    if (orient & 0x02)
+	printf("Orient->X Down\n");
+
+    if (orient & 0x04)
+	printf("Orient->Y Up\n");
+
+    if (orient & 0x08)
+	printf("Orient->Y Down\n");
+
+    if (orient & 0x10)
+	printf("Orient->Z Up\n");
+
+    if (orient & 0x20)
+	printf("Orient->Z Down\n");
+
+    if (orient & 0x40)
+	printf("Orient->Flip\n");
+
+    return 0;
+}
+
+int enableDMPFeatures(int en)
+{
+    int res= -1;
+
+    if (DmpFWloaded())
+    {
+        /* Currently there's no info regarding DMP's supported features/capabilities */
+        /* An error in enabling features below could be an indication of the feature */
+        /* not supported in current loaded DMP firmware */
+
+        enable_flick(en);
+        enable_tap(en);
+        enable_displ_orient(en);
+        enable_orient(en);
+        res= 0;
+    }
+
+    return res;
+}
+
+int initFds()
+{
+    int i;
+
+    for (i=0; i< numDMPFeatures; i++) {
+        switch(i) {
+            case tap:
+                pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case flick:
+                pfd[i].fd = open(mpu.event_flick, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case gOrient:
+                pfd[i].fd = open(mpu.event_display_orientation, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case orient:
+                pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
+                break;
+
+            default:
+                pfd[i].fd = -1;
+         }
+
+        pfd[i].events = POLLPRI|POLLERR,
+        pfd[i].revents = 0;
+#ifdef DEBUG_PRINT
+        printf("GT:pfd[%d].fd= %d\n", i, pfd[i].fd);
+#endif
+    }
+
+    return 0;
+}
+
+int closeFds()
+{
+    int i;
+    for (i = 0; i < numDMPFeatures; i++) {
+        if (!pfd[i].fd)
+            close(pfd[i].fd);
+    }
+    return 0;
+}
+
+/*******************************************************************************
+ *                       M a i n  S e l f  T e s t
+ ******************************************************************************/
+
+int main(int argc, char **argv)
+{
+    char data[4];
+    int i, res= 0;
+
+    res = inv_init_sysfs_attributes();
+    if (res) {
+        printf("GT:ERR-Can't allocate mem");
+        return -1;
+    }
+
+    /* On Gesture/DMP supported features */
+    enableDMPFeatures(1);
+
+    /* init Fds to poll for Gesture data */
+    initFds();
+
+    /* prompt user to make gesture and how to stop program */
+    printf("\n**Please make Gesture to see data.  Press any key to stop Prog**\n\n");
+
+    do {
+        for (i=0; i< numDMPFeatures; i++) {
+            read(pfd[i].fd, data, 4);
+        }
+
+        poll(pfd, numDMPFeatures, POLL_TIME);
+
+        for (i=0; i< numDMPFeatures; i++) {
+           if(pfd[i].revents != 0) {
+               switch(i) {
+                   case tap:
+                       tapHandler();
+                       break;
+
+                   case flick:
+                       flickHandler();
+                       break;
+
+                   case gOrient:
+                       googleOrientHandler();
+                       break;
+
+                   case orient:
+                       orientHandler();
+                       break;
+
+                   default:
+                       printf("GT:ERR-Not supported");
+                       break;
+               }
+               pfd[i].revents= 0;	//no need. reset anyway
+           }
+        }
+
+    } while (!_kbhit());
+
+    /* Off DMP features */
+    enableDMPFeatures(0);
+
+    /* release resources */
+    closeFds();
+    if (sysfs_names_ptr) {
+        free(sysfs_names_ptr);
+    }
+
+    printf("\nThank You!\n");
+
+    return res;
+}
+
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
deleted file mode 100644
index 5d52b21..0000000
--- a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
+++ /dev/null
diff --git a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
deleted file mode 100644
index 0936212..0000000
--- a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
+++ /dev/null
@@ -1,13 +0,0 @@
-#### 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
deleted file mode 100644
index 6fa9aab..0000000
--- a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
+++ /dev/null
@@ -1,485 +0,0 @@
-/*
- * input interface testing
- */
-#include <sys/time.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <time.h>
-#include <fcntl.h>
-#include <sys/ioctl.h>
-#include <linux/input.h>
-#include <linux/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include "linux/ml_sysfs_helper.h"
-#include "authenticate.h"
-#include "ml_load_dmp.h"
-
-#if 0
-struct input_event {
-       struct timeval time;
-       __u16 type;
-       __u16 code;
-       __s32 value;
-};
-#endif
-
-void HandleOrient(int orient)
-{    
-    if (orient & 0x01)
-        printf("INV_X_UP\n");
-    if (orient & 0x02) 
-        printf("INV_X_DOWN\n");
-    if (orient & 0x04) 
-        printf("INV_Y_UP\n");
-    if (orient & 0x08) 
-        printf("INV_Y_DOWN\n");
-    if (orient & 0x10) 
-        printf("INV_Z_UP\n");
-    if (orient & 0x20) 
-        printf("INV_Z_DOWN\n");
-    if (orient & 0x40) 
-        printf("INV_ORIENTATION_FLIP\n");
-}
-
-void HandleTap(int tap)
-{
-    int tap_dir = tap/8;
-    int tap_num = tap%8 + 1;
-
-    switch (tap_dir) {
-        case 1:
-            printf("INV_TAP_AXIS_X_POS\n");
-            break;
-        case 2:
-            printf("INV_TAP_AXIS_X_NEG\n");
-            break;
-        case 3:
-            printf("INV_TAP_AXIS_Y_POS\n");
-            break;
-        case 4:
-            printf("INV_TAP_AXIS_Y_NEG\n");
-            break;
-        case 5:
-            printf("INV_TAP_AXIS_Z_POS\n");
-            break;
-        case 6:
-            printf("INV_TAP_AXIS_Z_NEG\n");
-            break;
-        default:
-            break;
-    }
-    printf("Tap number: %d\n", tap_num);
-}
-
-static void read_compass(int event_number) 
-{
-    int ev_size, ret_byte, ii;
-    int fd = -1;
-    struct input_event ev[10];
-    char name[64];
-    char file_name[64];
-    unsigned int RX;
-    unsigned long long time0, time1, time2;
-    struct timespec tsp;
-    ev_size = sizeof(struct input_event);
-    sprintf(file_name, "/dev/input/event%d", event_number);
-    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
-        printf("fail to open compass\n");
-        return;
-    }
-
-    /* NOTE: Use this to pass device name to HAL. */
-    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
-    printf ("Reading From : (%s)\n", name);
-    while (1) {
-    clock_gettime(CLOCK_MONOTONIC, &tsp);
-        /*read compass data here */
-        if(fd > 0){
-            ret_byte = read(fd, ev, ev_size);
-        } else {
-            ret_byte = -1;
-        }
-        time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
-        if (ret_byte < 0)
-            continue;
-        for (ii = 0; ii < ret_byte/ev_size; ii++) {
-            if(EV_REL != ev[ii].type) {
-                time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
-                printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0));
-                continue;
-            }
-            switch (ev[ii].code) {
-            case REL_X:
-                printf("CX:%5d ", ev[ii].value);
-                break;
-            case REL_Y:
-                printf("CY:%5d ", ev[ii].value);
-                break;
-            case REL_Z:
-                printf("CZ:%5d ", ev[ii].value);
-                break;
-            case REL_MISC:
-                RX = ev[ii].value;
-                break;
-            case REL_WHEEL:
-                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
-                time1 = time1/1000000;
-                printf("time1: %lld ", time1);
-                break;
-            default:
-                printf("GES?:  %5d ", ev[ii].code);
-                break;
-            }
-        }
-    }
-    close(fd);
-}
-
-static void read_gesture(int num)
-{
-    int ev_size, ret_byte, ii;
-    int fd = -1;
-    struct input_event ev[10];
-    char name[64];
-    char file_name[64];
-    unsigned long long time;
-    struct timespec tsp;
-    ev_size = sizeof(struct input_event);
-    sprintf(file_name, "/dev/input/event%d", num);
-    MPL_LOGI("%s\n", file_name);
-    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
-        printf("fail to open gusture.\n");
-        return;
-    }
-
-    /* NOTE: Use this to pass device name to HAL. */
-    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
-    printf ("Reading From : (%s)\n", name);
-    while(1){
-        clock_gettime(CLOCK_MONOTONIC, &tsp);
-        if(fd > 0){
-            ret_byte = read(fd, ev, ev_size);
-        } else {
-            ret_byte = -1;
-        }
-        time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL;
-        //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3);
-        if (ret_byte < 0)
-            continue;
-        for (ii = 0; ii < ret_byte/ev_size; ii++) {
-            if(EV_REL != ev[ii].type) {
-                time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL;
-                printf("mono=%lld\n", time);
-                continue;
-            }
-            switch (ev[ii].code) {
-            case REL_RX:
-                printf("GESX:%5x\n", ev[ii].value);
-                HandleTap(ev[ii].value);
-                break;
-            case REL_RY:
-                printf("GESY:%5x\n", ev[ii].value);
-                HandleOrient(ev[ii].value);
-                break;
-            case REL_RZ:
-                printf("FLICK:%5x\n", ev[ii].value);
-                break;
-            default:
-                printf("?:  %5d ", ev[ii].code);
-                break;
-            }
-        }
-        }
-}
-
-static void read_gyro_accel(int num)
-{
-    int ev_size, ret_byte, ii;
-    int fd = -1;
-    unsigned int RX;
-    struct input_event ev[10];
-    char name[64];
-    char file_name[64];
-    unsigned long long time0, time1, time2;
-    struct timespec tsp;
-    ev_size = sizeof(struct input_event);
-    sprintf(file_name, "/dev/input/event%d", num);
-    if ((fd = open(file_name, O_RDONLY)) < 0 ) {
-        printf("fail to open gyro/accel\n");
-        return;
-    }    
-        
-    /* NOTE: Use this to pass device name to HAL. */
-    ioctl (fd, EVIOCGNAME (sizeof (name)), name);
-    printf ("Reading From : (%s)\n", name);
-    while (1){
-        //usleep(20000);
-        ret_byte = read(fd, ev, ev_size);
-        if (ret_byte < 0)
-            continue;
-        //ret_byte = 0;
-
-        for (ii = 0; ii < ret_byte/ev_size; ii++) {
-            if(EV_REL != ev[ii].type) {
-                time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
-                printf("T: %lld diff=%d ", time0, (int)(time1 - time0));
-                clock_gettime(CLOCK_MONOTONIC, &tsp);
-                time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
-                printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2));
-                continue;
-            }
-            switch (ev[ii].code) {
-            case REL_X:
-                printf("GX:%5d ", ev[ii].value);
-                break;
-            case REL_Y:
-                printf("GY:%5d ", ev[ii].value);
-                break;
-            case REL_Z:
-                printf("GZ:%5d ", ev[ii].value);
-                break;
-            case REL_RX:
-                printf("AX:%5d ", ev[ii].value);
-                break;
-            case REL_RY:
-                printf("AY:%5d ", ev[ii].value);
-                break;
-            case REL_RZ:
-                printf("AZ:%5d ", ev[ii].value);
-                break;
-            case REL_MISC:
-                RX = ev[ii].value;
-                break;
-            case REL_WHEEL:
-                time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
-                time1 = time1/1000000;
-                printf("time1: %lld ", time1);
-                break;
-            default:
-                printf("?:  %5d ", ev[ii].code);
-                break;
-            }
-        }
-    }
-    close(fd);
-}
-int inv_sysfs_write(char *filename, long data)
-{
-    FILE *fp;
-    int count;
-
-    if (!filename)
-        return -1;
-    fp = fopen(filename, "w");
-    if (!fp)
-        return -errno;
-    count = fprintf(fp, "%ld", data);
-    fclose(fp);
-    return count;
-}
-int inv_sysfs_read(char *filename, long num_bytes, char *data)
-{
-    FILE *fp;
-    int count;
-
-    if (!filename)
-        return -1;
-    fp = fopen(filename, "r");
-    if (!fp)
-        return -errno;
-    count = fread(data, 1, num_bytes, fp);
-    fclose(fp);
-    return count;
-}
-
-void enable_flick(char *p)
-{
-    char sysfs_file[200];
-    printf("flick:%s\n", p);
-    sprintf(sysfs_file, "%s/flick_int_on", p);
-    inv_sysfs_write(sysfs_file, 1);
-    sprintf(sysfs_file, "%s/flick_upper", p);
-    inv_sysfs_write(sysfs_file, 3147790);
-    sprintf(sysfs_file, "%s/flick_lower", p);
-    inv_sysfs_write(sysfs_file, -3147790);
-    sprintf(sysfs_file, "%s/flick_counter", p);
-    inv_sysfs_write(sysfs_file, 50);
-    sprintf(sysfs_file, "%s/flick_message_on", p);
-    inv_sysfs_write(sysfs_file, 0);
-    sprintf(sysfs_file, "%s/flick_axis", p);
-    inv_sysfs_write(sysfs_file, 2);
-}
-
-void setup_dmp(char *sysfs_path)
-{
-    char sysfs_file[200];
-    char firmware_loaded[200], dmp_path[200];
-    char dd[10];
-    
-    inv_get_dmpfile(dmp_path);
-    sprintf(sysfs_file, "%s/fifo_rate", sysfs_path);
-    inv_sysfs_write(sysfs_file, 200);    
-    sprintf(sysfs_file, "%s/FSR", sysfs_path);
-    inv_sysfs_write(sysfs_file, 2000);    
-    sprintf(sysfs_file, "%s/accl_fs", sysfs_path);
-    inv_sysfs_write(sysfs_file, 4);
-    /*
-    sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded");
-    printf("%s\n", firmware_loaded);
-    inv_sysfs_write(firmware_loaded, 0);
-    inv_sysfs_read(firmware_loaded, 1, dd);
-    printf("beforefirmware_loaded=%c\n", dd[0]);
-
-    if ((fd = open(dmp_path, O_WRONLY)) < 0 ) {
-        perror("dmp fail");
-    }
-    inv_load_dmp(fd);
-    close(fd);
-    */
-    inv_sysfs_read(firmware_loaded, 1, dd);
-    printf("firmware_loaded=%c\n", dd[0]);
-}
-void read_pedometer(char *sysfs_path){
-    int steps;
-    char sysfs_file[200];
-    char dd[4];
-    sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path);
-    inv_sysfs_read(sysfs_file, 4, dd);
-    steps = dd[0] << 8 | dd[1];
-    printf("fff=%d\n", steps);
-}
-/* The running sequence:
-    "input_gyro 2 &". 
-    This will setup the dmp firmware and let it run on background. 
-    tap and flick will work at this time. 
-    To see accelerometer data and gyro data.
-    type :
-    "input_gyro ". 
-    This will print out gyro data and accelerometer data
-    To see Compass data
-    type:
-    "input_gyro 1" */
-
-int main(int argc, char *argv[])
-{
-    unsigned int RX, i, sel;
-    unsigned char key[16];
-    struct timeval tv;
-    struct timespec tsp0, tsp1, tsp2, tsp3;
-    int event_num;
-    char sysfs_path[200];
-    char chip_name[20];
-    char sysfs_file[200];
-    if (INV_SUCCESS != inv_check_key()) {
-        printf("key check fail\n");
-        exit(0);
-    }else
-        printf("key authenticated\n");
-
-    for(i=0;i<16;i++){
-        key[i] = 0xff;
-    }
-    RX = inv_get_sysfs_key(key);
-    if(RX == INV_SUCCESS){
-        for(i=0;i<16;i++){
-            printf("%d, ", key[i]);
-        }
-        printf("\n");
-    }else{
-        printf("get key failed\n");
-    }
-    memset(sysfs_path, 0, 200);
-    memset(sysfs_file, 0, 200);
-    memset(chip_name, 0, 20);
-    inv_get_sysfs_path(sysfs_path);
-    inv_get_chip_name(chip_name);
-    printf("sysfs path: %s\n", sysfs_path);
-    printf("chip name: %s\n", chip_name);
-    /*set up driver*/
-    sprintf(sysfs_file, "%s/enable", sysfs_path);
-    inv_sysfs_write(sysfs_file, 0);
-    sprintf(sysfs_file, "%s/power_state", sysfs_path);
-    inv_sysfs_write(sysfs_file, 1);    
-    if ((getuid ()) != 0)
-        printf ("You are not root! This may not work...\n");
-    
-    if(argc ==2 )
-        sel = argv[1][0] - 0x30;
-    else
-        sel = 0;
-    switch(sel){
-    case 0:
-        printf("-------------------------------\n");
-        printf("--- log gyro and accel data ---\n");
-        printf("-------------------------------\n");
-        sprintf(sysfs_file, "%s/enable", sysfs_path);
-        inv_sysfs_write(sysfs_file, 1);
-        if(inv_get_handler_number(chip_name, &event_num) < 0)
-            printf("mpu not installed\n");
-        else
-            read_gyro_accel(event_num);
-         break;
-         
-    case 1:
-        printf("------------------------\n");
-        printf("--- log compass data ---\n");
-        printf("------------------------\n");
-        sprintf(sysfs_file, "%s/compass_enable", sysfs_path);
-        inv_sysfs_write(sysfs_file, 1);
-        sprintf(sysfs_file, "%s/enable", sysfs_path);
-        inv_sysfs_write(sysfs_file, 1);
-        if(inv_get_handler_number("INV_COMPASS", &event_num) < 0)
-            printf("compass is not enabled\n");
-        else
-            read_compass(event_num);
-        break;
-        
-    case 2:
-        printf("--------------------\n");
-        printf("--- log gestures ---\n");
-        printf("--------------------\n");
-        setup_dmp(sysfs_path);
-        enable_flick(sysfs_path);
-        sprintf(sysfs_file, "%s/tap_on", sysfs_path);
-        inv_sysfs_write(sysfs_file, 1);
-        sprintf(sysfs_file, "%s/enable", sysfs_path);
-        inv_sysfs_write(sysfs_file, 1);
-        if(inv_get_handler_number("INV_DMP", &event_num) < 0)
-            printf("DMP not enabled\n");
-        else
-            read_gesture(event_num);
-        break;
-        
-    case 3:
-        printf("-----------------\n");
-        printf("--- pedometer ---\n");
-        printf("-----------------\n");
-        read_pedometer(sysfs_path);
-        break;
-        
-    default:
-        printf("error choice\n");
-        break;
-    }
-    
-    gettimeofday(&tv, NULL);
-    clock_gettime(CLOCK_MONOTONIC, &tsp1);
-    clock_gettime(CLOCK_REALTIME, &tsp0);
-
-    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2);
-    clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3);
-    //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC,  CLOCK_REALTIME,
-    //    CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID);
-    //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec);
-    //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec);
-    //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec);
-    //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec);
-
-    //ioctl (fd, EVIOCGNAME (sizeof (name)), name);
-    //printf ("Reading From : %s (%s)\n", argv[1], name);
-
-
-    return 0;
-}
-
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
new file mode 100644
index 0000000..14ca523
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
index b1d881c..cd79cfa 100644
--- a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
+++ b/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
@@ -1,4 +1,4 @@
-EXEC = consoledmp$(SHARED_APP_SUFFIX)
+EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX)
 
 MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
 
@@ -8,16 +8,15 @@ LINK  ?= $(CROSS)gcc
 
 OBJFOLDER = $(CURDIR)/obj
 
-INV_ROOT   = ../../../../../..
+INV_ROOT   = ../../../../..
 APP_DIR    = $(CURDIR)/../..
 MLLITE_DIR = $(INV_ROOT)/software/core/mllite
-COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
 MPL_DIR    = $(INV_ROOT)/software/core/mpl
-HAL_DIR    = $(INV_ROOT)/software/core/HAL
 
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -49,20 +48,8 @@ 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
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
 
 LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
@@ -87,7 +74,7 @@ all: $(EXEC) $(MK_NAME)
 
 $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
 	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
 
 $(OBJFOLDER) :
 	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
new file mode 100644
index 0000000..8a3977a
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
@@ -0,0 +1,12 @@
+#### filelist.mk for mpu_iio ####
+
+# headers
+#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+HEADERS += $(APP_DIR)/iio_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/mpu_iio.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
new file mode 100644
index 0000000..773ff2c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
@@ -0,0 +1,643 @@
+/* IIO - useful set of util functionality
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/* Made up value to limit allocation sizes */
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+
+#define IIO_MAX_NAME_LENGTH 30
+
+#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
+#define FORMAT_TYPE_FILE "%s_type"
+
+const char *iio_dir = "/sys/bus/iio/devices/";
+
+/**
+ * iioutils_break_up_name() - extract generic name from full channel name
+ * @full_name: the full channel name
+ * @generic_name: the output generic channel name
+ **/
+static int iioutils_break_up_name(const char *full_name,
+				  char **generic_name)
+{
+	char *current;
+	char *w, *r;
+	char *working;
+	current = strdup(full_name);
+	working = strtok(current, "_\0");
+	w = working;
+	r = working;
+
+	while (*r != '\0') {
+		if (!isdigit(*r)) {
+			*w = *r;
+			w++;
+		}
+		r++;
+	}
+	*w = '\0';
+	*generic_name = strdup(working);
+	free(current);
+
+	return 0;
+}
+
+/**
+ * struct iio_channel_info - information about a given channel
+ * @name: channel name
+ * @generic_name: general name for channel type
+ * @scale: scale factor to be applied for conversion to si units
+ * @offset: offset to be applied for conversion to si units
+ * @index: the channel index in the buffer output
+ * @bytes: number of bytes occupied in buffer output
+ * @mask: a bit mask for the raw output
+ * @is_signed: is the raw value stored signed
+ * @enabled: is this channel enabled
+ **/
+struct iio_channel_info {
+	char *name;
+	char *generic_name;
+	float scale;
+	float offset;
+	unsigned index;
+	unsigned bytes;
+	unsigned bits_used;
+	unsigned shift;
+	uint64_t mask;
+	unsigned be;
+	unsigned is_signed;
+	unsigned enabled;
+	unsigned location;
+};
+
+/**
+ * iioutils_get_type() - find and process _type attribute data
+ * @is_signed: output whether channel is signed
+ * @bytes: output how many bytes the channel storage occupies
+ * @mask: output a bit mask for the raw data
+ * @be: big endian
+ * @device_dir: the iio device directory
+ * @name: the channel name
+ * @generic_name: the channel type name
+ **/
+inline int iioutils_get_type(unsigned *is_signed,
+			     unsigned *bytes,
+			     unsigned *bits_used,
+			     unsigned *shift,
+			     uint64_t *mask,
+			     unsigned *be,
+			     const char *device_dir,
+			     const char *name,
+			     const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
+	char signchar, endianchar;
+	unsigned padint;
+	const struct dirent *ent;
+
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_scan_el_dir;
+	}
+	ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		/*
+		 * Do we allow devices to override a generic name with
+		 * a specific one?
+		 */
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				printf("failed to open %s\n", filename);
+				ret = -errno;
+				goto error_free_filename;
+			}
+
+			ret = fscanf(sysfsfp,
+				     "%ce:%c%u/%u>>%u",
+				     &endianchar,
+				     &signchar,
+				     bits_used,
+				     &padint, shift);
+			if (ret < 0) {
+				printf("failed to pass scan type description\n");
+				return ret;
+			}
+			*be = (endianchar == 'b');
+			*bytes = padint / 8;
+			if (*bits_used == 64)
+				*mask = ~0;
+			else
+				*mask = (1 << *bits_used) - 1;
+			if (signchar == 's')
+				*is_signed = 1;
+			else
+				*is_signed = 0;
+			fclose(sysfsfp);
+			free(filename);
+
+			filename = 0;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_free_scan_el_dir:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int iioutils_get_param_float(float *output,
+				    const char *param_name,
+				    const char *device_dir,
+				    const char *name,
+				    const char *generic_name)
+{
+	FILE *sysfsfp;
+	int ret;
+	DIR *dp;
+	char *builtname, *builtname_generic;
+	char *filename = NULL;
+	const struct dirent *ent;
+
+	ret = asprintf(&builtname, "%s_%s", name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	ret = asprintf(&builtname_generic,
+		       "%s_%s", generic_name, param_name);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_free_builtname;
+	}
+	dp = opendir(device_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_builtname_generic;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if ((strcmp(builtname, ent->d_name) == 0) ||
+		    (strcmp(builtname_generic, ent->d_name) == 0)) {
+			ret = asprintf(&filename,
+				       "%s/%s", device_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_closedir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (!sysfsfp) {
+				ret = -errno;
+				goto error_free_filename;
+			}
+			fscanf(sysfsfp, "%f", output);
+			break;
+		}
+error_free_filename:
+	if (filename)
+		free(filename);
+error_closedir:
+	closedir(dp);
+error_free_builtname_generic:
+	free(builtname_generic);
+error_free_builtname:
+	free(builtname);
+error_ret:
+	return ret;
+}
+
+/**
+ * bsort_channel_array_by_index() - reorder so that the array is in index order
+ *
+ **/
+
+inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
+					 int cnt)
+{
+
+	struct iio_channel_info temp;
+	int x, y;
+
+	for (x = 0; x < cnt; x++)
+		for (y = 0; y < (cnt - 1); y++)
+			if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
+				temp = (*ci_array)[y + 1];
+				(*ci_array)[y + 1] = (*ci_array)[y];
+				(*ci_array)[y] = temp;
+			}
+}
+
+/**
+ * build_channel_array() - function to figure out what channels are present
+ * @device_dir: the IIO device directory in sysfs
+ * @
+ **/
+inline int build_channel_array(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter)
+{
+	DIR *dp;
+	FILE *sysfsfp;
+	int count, i;
+	struct iio_channel_info *current;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+	char *filename;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_close_dir;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				ret = -errno;
+				free(filename);
+				goto error_close_dir;
+			}
+			fscanf(sysfsfp, "%u", &ret);
+			printf("%s, %d\n", filename, ret);
+			if (ret == 1)
+				(*counter)++;
+			fclose(sysfsfp);
+			free(filename);
+		}
+	*ci_array = malloc(sizeof(**ci_array) * (*counter));
+	if (*ci_array == NULL) {
+		ret = -ENOMEM;
+		goto error_close_dir;
+	}
+	closedir(dp);
+	dp = opendir(scan_el_dir);
+	//seekdir(dp, 0);
+	count = 0;
+	while (ent = readdir(dp), ent != NULL) {
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			current = &(*ci_array)[count++];
+			ret = asprintf(&filename,
+				       "%s/%s", scan_el_dir, ent->d_name);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				/* decrement count to avoid freeing name */
+				count--;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			if (sysfsfp == NULL) {
+				free(filename);
+				ret = -errno;
+				goto error_cleanup_array;
+			}
+			fscanf(sysfsfp, "%u", &current->enabled);
+			fclose(sysfsfp);
+
+			if (!current->enabled) {
+				free(filename);
+				count--;
+				continue;
+			}
+
+			current->scale = 1.0;
+			current->offset = 0;
+			current->name = strndup(ent->d_name,
+						strlen(ent->d_name) -
+						strlen("_en"));
+			if (current->name == NULL) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			/* Get the generic and specific name elements */
+			ret = iioutils_break_up_name(current->name,
+						     &current->generic_name);
+			if (ret) {
+				free(filename);
+				goto error_cleanup_array;
+			}
+			ret = asprintf(&filename,
+				       "%s/%s_index",
+				       scan_el_dir,
+				       current->name);
+			if (ret < 0) {
+				free(filename);
+				ret = -ENOMEM;
+				goto error_cleanup_array;
+			}
+			sysfsfp = fopen(filename, "r");
+			fscanf(sysfsfp, "%u", &current->index);
+			fclose(sysfsfp);
+			free(filename);
+			/* Find the scale */
+			ret = iioutils_get_param_float(&current->scale,
+						       "scale",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_param_float(&current->offset,
+						       "offset",
+						       device_dir,
+						       current->name,
+						       current->generic_name);
+			if (ret < 0)
+				goto error_cleanup_array;
+			ret = iioutils_get_type(&current->is_signed,
+						&current->bytes,
+						&current->bits_used,
+						&current->shift,
+						&current->mask,
+						&current->be,
+						device_dir,
+						current->name,
+						current->generic_name);
+		}
+	}
+
+	closedir(dp);
+	/* reorder so that the array is in index order */
+	bsort_channel_array_by_index(ci_array, *counter);
+
+	return 0;
+
+error_cleanup_array:
+	for (i = count - 1;  i >= 0; i--)
+		free((*ci_array)[i].name);
+	free(*ci_array);
+error_close_dir:
+	closedir(dp);
+error_free_name:
+	free(scan_el_dir);
+error_ret:
+	return ret;
+}
+
+inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
+{
+	int ret;
+	FILE *sysfsfp;
+	int test;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL)
+		return -ENOMEM;
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("failed to open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%d", val);
+	fclose(sysfsfp);
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("failed to open %s\n", temp);
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%d", &test);
+		if (test != val) {
+			printf("Possible failure in int write %d to %s%s\n",
+				val,
+				basedir,
+				filename);
+			ret = -1;
+		}
+	}
+error_free:
+	free(temp);
+	return ret;
+}
+
+int write_sysfs_int(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 0);
+}
+
+int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
+{
+	return _write_sysfs_int(filename, basedir, val, 1);
+}
+
+int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
+{
+	int ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed\n");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "w");
+	if (sysfsfp == NULL) {
+		printf("Could not open %s\n", temp);
+		ret = -errno;
+		goto error_free;
+	}
+	fprintf(sysfsfp, "%s", val);
+	fclose(sysfsfp);
+	if (verify) {
+		sysfsfp = fopen(temp, "r");
+		if (sysfsfp == NULL) {
+			printf("could not open file to verify\n");
+			ret = -errno;
+			goto error_free;
+		}
+		fscanf(sysfsfp, "%s", temp);
+		if (strcmp(temp, val) != 0) {
+			printf("Possible failure in string write of %s "
+				"Should be %s "
+				"written to %s\%s\n",
+				temp,
+				val,
+				basedir,
+				filename);
+			ret = -1;
+		}
+	}
+error_free:
+	free(temp);
+
+	return ret;
+}
+
+/**
+ * write_sysfs_string_and_verify() - string write, readback and verify
+ * @filename: name of file to write to
+ * @basedir: the sysfs directory in which the file is to be found
+ * @val: the string to write
+ **/
+int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 1);
+}
+
+int write_sysfs_string(char *filename, char *basedir, char *val)
+{
+	return _write_sysfs_string(filename, basedir, val, 0);
+}
+
+int read_sysfs_posint(char *filename, char *basedir)
+{
+	int ret;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%d\n", &ret);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+
+int read_sysfs_float(char *filename, char *basedir, float *val)
+{
+	float ret = 0;
+	FILE  *sysfsfp;
+	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+	if (temp == NULL) {
+		printf("Memory allocation failed");
+		return -ENOMEM;
+	}
+	sprintf(temp, "%s/%s", basedir, filename);
+	sysfsfp = fopen(temp, "r");
+	if (sysfsfp == NULL) {
+		ret = -errno;
+		goto error_free;
+	}
+	fscanf(sysfsfp, "%f\n", val);
+	fclose(sysfsfp);
+error_free:
+	free(temp);
+	return ret;
+}
+int enable(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter)
+{
+	DIR *dp;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
+			   "_en") == 0) {
+			write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 1);
+		}
+	return 0;
+error_ret:
+error_free_name:
+	return -1;
+}
+int disable_q_out(const char *device_dir,
+			      struct iio_channel_info **ci_array,
+			      int *counter) {
+	DIR *dp;
+	int ret;
+	const struct dirent *ent;
+	char *scan_el_dir;
+
+	*counter = 0;
+	ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
+	if (ret < 0) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+	dp = opendir(scan_el_dir);
+	if (dp == NULL) {
+		ret = -errno;
+		goto error_free_name;
+	}
+	while (ent = readdir(dp), ent != NULL)
+		if (strncmp(ent->d_name, "in_quaternion", strlen("in_quaternion")) == 0) {
+			write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 0);
+		}
+	return 0;
+error_ret:
+error_free_name:
+	return -1;
+
+}
+
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
new file mode 100644
index 0000000..b3d323c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
@@ -0,0 +1,685 @@
+/* Industrialio buffer test code.
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is primarily intended as an example application.
+ * Reads the current buffer setup from sysfs and starts a short capture
+ * from the specified device, pretty printing the result after appropriate
+ * conversion.
+ *
+ * Command line parameters
+ * generic_buffer -n <device_name> -t <trigger_name>
+ * If trigger name is not specified the program assumes you want a dataready
+ * trigger associated with the device and goes looking for it.
+ *
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <linux/types.h>
+#include <string.h>
+#include <poll.h>
+#include "iio_utils.h"
+#include "ml_load_dmp.h"
+#include "ml_sysfs_helper.h"
+#include "authenticate.h"
+
+#define FLICK_SUPPORTED (0)
+
+/**
+ * size_from_channelarray() - calculate the storage size of a scan
+ * @channels: the channel info array
+ * @num_channels: size of the channel info array
+ *
+ * Has the side effect of filling the channels[i].location values used
+ * in processing the buffer output.
+ **/
+int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
+{
+    int bytes = 0;
+    int i = 0;
+    while (i < num_channels) {
+        if (bytes % channels[i].bytes == 0)
+            channels[i].location = bytes;
+        else
+            channels[i].location = bytes - bytes%channels[i].bytes
+                + channels[i].bytes;
+        bytes = channels[i].location + channels[i].bytes;
+        i++;
+    }
+    return bytes;
+}
+
+void print2byte(int input, struct iio_channel_info *info)
+{
+    /* shift before conversion to avoid sign extension
+       of left aligned data */
+    input = input >> info->shift;
+    if (info->is_signed) {
+        int16_t val = input;
+        val &= (1 << info->bits_used) - 1;
+        val = (int16_t)(val << (16 - info->bits_used)) >>
+            (16 - info->bits_used);
+        /*printf("%d, %05f, scale=%05f", val,
+               (float)(val + info->offset)*info->scale, info->scale);*/
+        printf("%d, ", val);
+
+    } else {
+        uint16_t val = input;
+        val &= (1 << info->bits_used) - 1;
+        printf("%05f ", ((float)val + info->offset)*info->scale);
+    }
+}
+/**
+ * process_scan() - print out the values in SI units
+ * @data:        pointer to the start of the scan
+ * @infoarray:        information about the channels. Note
+ *  size_from_channelarray must have been called first to fill the
+ *  location offsets.
+ * @num_channels:    the number of active channels
+ **/
+void process_scan(char *data,
+          struct iio_channel_info *infoarray,
+          int num_channels)
+{
+    int k;
+    //char *tmp;
+    for (k = 0; k < num_channels; k++) {
+        switch (infoarray[k].bytes) {
+            /* only a few cases implemented so far */
+        case 2:
+            print2byte(*(uint16_t *)(data + infoarray[k].location),
+                   &infoarray[k]);
+            //tmp = data + infoarray[k].location;
+            break;
+        case 4:
+            if (infoarray[k].is_signed) {
+                int32_t val = *(int32_t *)
+                    (data +
+                     infoarray[k].location);
+                if ((val >> infoarray[k].bits_used) & 1)
+                    val = (val & infoarray[k].mask) |
+                        ~infoarray[k].mask;
+                /* special case for timestamp */
+                printf(" %d ", val);
+            }
+            break;
+        case 8:
+            if (infoarray[k].is_signed) {
+                int64_t val = *(int64_t *)
+                    (data +
+                     infoarray[k].location);
+                if ((val >> infoarray[k].bits_used) & 1)
+                    val = (val & infoarray[k].mask) |
+                        ~infoarray[k].mask;
+                /* special case for timestamp */
+                if (infoarray[k].scale == 1.0f &&
+                    infoarray[k].offset == 0.0f)
+                    printf(" %lld", val);
+                else
+                    printf("%05f ", ((float)val +
+                             infoarray[k].offset)*
+                           infoarray[k].scale);
+            }
+            break;
+        default:
+            break;
+        }
+    }
+    printf("\n");
+}
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+void enable_flick(char *p, int on){
+    int ret;
+    printf("flick:%s\n", p);
+    ret = write_sysfs_int_and_verify("flick_int_on", p, on);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_upper", p, 3147790);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_lower", p, -3147790);
+    if (ret < 0)
+        return;
+
+    ret = write_sysfs_int_and_verify("flick_counter", p, 50);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_message_on", p, 0);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_axis", p, 0);
+}
+#endif
+
+void HandleOrient(int orient)
+{    
+    if (orient & 0x01)
+    printf("INV_X_UP\n");
+    if (orient & 0x02) 
+    printf("INV_X_DOWN\n");
+    if (orient & 0x04) 
+    printf("INV_Y_UP\n");
+    if (orient & 0x08) 
+    printf("INV_Y_DOWN\n");
+    if (orient & 0x10) 
+    printf("INV_Z_UP\n");
+    if (orient & 0x20) 
+    printf("INV_Z_DOWN\n");
+    if (orient & 0x40) 
+    printf("INV_ORIENTATION_FLIP\n");
+}
+
+void HandleTap(int tap)
+{
+    int tap_dir = tap/8;
+    int tap_num = tap%8 + 1;
+
+    switch (tap_dir) {
+        case 1:
+            printf("INV_TAP_AXIS_X_POS\n");
+            break;
+        case 2:
+            printf("INV_TAP_AXIS_X_NEG\n");
+            break;
+        case 3:
+            printf("INV_TAP_AXIS_Y_POS\n");
+            break;
+        case 4:
+            printf("INV_TAP_AXIS_Y_NEG\n");
+            break;
+        case 5:
+            printf("INV_TAP_AXIS_Z_POS\n");
+            break;
+        case 6:
+            printf("INV_TAP_AXIS_Z_NEG\n");
+            break;
+        default:
+            break;
+    }
+    printf("Tap number: %d\n", tap_num);
+}
+#define DMP_CODE_SIZE 3060
+void verify_img(char *dmp_path){
+    FILE *fp;
+    int i;
+    char dmp_img[DMP_CODE_SIZE];
+    if ((fp = fopen(dmp_path, "rb")) < 0 ) {
+        perror("dmp fail");
+    }
+    i = fread(dmp_img, 1, DMP_CODE_SIZE, fp);
+    printf("Result=%d\n", i);
+    fclose(fp);
+    fp = fopen("/dev/read_img.h", "wt");
+    fprintf(fp, "char rec[]={\n");
+    for(i=0; i<DMP_CODE_SIZE; i++) {
+      fprintf(fp, "0x%02x, ", dmp_img[i]);
+      if(((i+1)%16) == 0) {
+        fprintf(fp, "\n");
+      }
+    }
+    fprintf(fp, "};\n ");
+    fclose(fp);
+}
+
+void setup_dmp(char *dev_path, int p_event){
+    char sysfs_path[200];
+    char dmp_path[200];
+    int  ret;
+    FILE *fd;
+    sprintf(sysfs_path, "%s", dev_path);
+    printf("sysfs: %s\n", sysfs_path);
+    ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1);
+    if (ret < 0)
+        return;
+
+	ret = write_sysfs_int("in_accel_scale", dev_path, 0);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int("in_anglvel_scale", dev_path, 2);
+	if (ret < 0)
+		return;	
+	ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
+	if (ret < 0)
+		return;	
+	ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0);
+	if (ret < 0)
+		return;
+	sprintf(dmp_path, "%s/dmp_firmware", dev_path);
+	if ((fd = fopen(dmp_path, "wb")) < 0 ) {
+		perror("dmp fail");
+	}	
+	inv_load_dmp(fd);
+	fclose(fd);
+	printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
+	ret = write_sysfs_int_and_verify("in_accel_x_offset", sysfs_path, 0xabcd0000);
+	ret = write_sysfs_int_and_verify("in_accel_y_offset", sysfs_path, 0xffff0000);
+	ret = write_sysfs_int_and_verify("in_accel_z_offset", sysfs_path, 0xcdef0000);
+
+	ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	/* selelct which event to enable and interrupt on/off here */
+	//enable_flick(sysfs_path, 1);
+	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1);
+	if (ret < 0)
+		return;
+	printf("rate\n");
+	ret = write_sysfs_int_and_verify("dmp_output_rate", sysfs_path, 25);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, p_event);
+	if (ret < 0)
+		return;
+	//verify_img(dmp_path);
+}
+
+void get_dmp_event(char *dev_dir_name)
+{
+    char file_name[100];
+    int i;
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+    int fp_tap, fp_orient, fp_disp, fp_flick;
+    const int n_gest = 6;
+#else
+    int fp_tap, fp_orient, fp_disp, fp_motion;
+    //int fp_no_motion;
+    const int n_gest = 4;
+#endif
+    int data;
+    char d[6];
+    FILE *fp;
+    struct pollfd pfd[4];
+    printf("%s\n", dev_dir_name);
+    while(1) {
+        sprintf(file_name, "%s/event_tap", dev_dir_name);
+        fp_tap = open(file_name, O_RDONLY | O_NONBLOCK);
+        sprintf(file_name, "%s/event_orientation", dev_dir_name);
+        fp_orient = open(file_name, O_RDONLY | O_NONBLOCK);
+        sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
+        fp_disp = open(file_name, O_RDONLY | O_NONBLOCK);
+
+        //sprintf(file_name, "%s/event_accel_motion", dev_dir_name);
+        sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
+        fp_motion = open(file_name, O_RDONLY | O_NONBLOCK);
+        //sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
+        //fp_no_motion = open(file_name, O_RDONLY | O_NONBLOCK);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        sprintf(file_name, "%s/event_flick", dev_dir_name);
+        fp_flick = open(file_name, O_RDONLY | O_NONBLOCK);
+#endif
+
+        pfd[0].fd = fp_tap;
+        pfd[0].events = POLLPRI|POLLERR,
+        pfd[0].revents = 0;
+
+        pfd[1].fd = fp_orient;
+        pfd[1].events = POLLPRI|POLLERR,
+        pfd[1].revents = 0;
+
+        pfd[2].fd = fp_disp;
+        pfd[2].events = POLLPRI|POLLERR,
+        pfd[2].revents = 0;
+
+        pfd[3].fd = fp_motion;
+        pfd[3].events = POLLPRI|POLLERR,
+        pfd[3].revents = 0;
+
+        //pfd[4].fd = fp_no_motion;
+        //pfd[4].events = POLLPRI|POLLERR,
+        //pfd[4].revents = 0;
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        pfd[5].fd = fp_flick;
+        pfd[5].events = POLLPRI|POLLERR,
+        pfd[5].revents = 0;    
+#endif
+
+        read(fp_tap, d, 4);
+        read(fp_orient, d, 4);
+        read(fp_disp, d, 4);
+        read(fp_motion, d, 4);
+        //read(fp_no_motion, d, 4);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        read(fp_flick, d, 4);
+#endif
+
+        poll(pfd, n_gest, -1);
+        close(fp_tap);
+        close(fp_orient);
+        close(fp_disp);
+        close(fp_motion);
+        //close(fp_no_motion);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        close(fp_flick);
+#endif
+        for (i = 0; i < ARRAY_SIZE(pfd); i++) {
+            if(pfd[i].revents != 0) {
+                switch (i){
+                case 0:
+                    sprintf(file_name, "%s/event_tap", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("tap=%x\n", data);
+                    HandleTap(data);
+                    fclose(fp);
+                break;
+                case 1:
+                    sprintf(file_name, "%s/event_orientation", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("orient=%x\n", data);
+                    HandleOrient(data);
+                    fclose(fp);
+                break;
+                case 2:
+                    sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("display_orient=%x\n", data);
+                    fclose(fp);
+                break;
+                case 3:
+                    sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("motion=%x\n", data);
+                    fclose(fp);
+                break;
+                case 4:
+                    sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("No motion=%x\n", data);
+                    fclose(fp);
+                break;
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+                case 5:
+                    sprintf(file_name, "%s/event_flick", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("flick=%x\n", data);
+                    fclose(fp);
+                break;
+#endif
+                }
+            }
+        }                        
+    }
+}
+
+
+int main(int argc, char **argv)
+{
+    unsigned long num_loops = 2;
+    unsigned long timedelay = 100000;
+    unsigned long buf_len = 128;
+
+    int ret, c, i, j, toread;
+    int fp;
+
+    int num_channels;
+    char *trigger_name = NULL;
+    char *dev_dir_name, *buf_dir_name;
+
+    int datardytrigger = 1;
+    char *data;
+    int read_size;
+    int dev_num, trig_num;
+    char *buffer_access;
+    int scan_size;
+    int noevents = 0;
+    int p_event = 0, nodmp = 0;
+    char *dummy;
+    char chip_name[10];
+    char device_name[10];
+    char sysfs[100];
+
+    struct iio_channel_info *infoarray;
+    /* -r means no DMP is enabled (raw) -> should be used for mpu3050.
+       -p means no print of data */
+    /* when using -p, 1 means orientation, 2 means tap, 3 means flick */
+    while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) {
+        switch (c) {
+        case 't':
+            trigger_name = optarg;
+            datardytrigger = 0;
+            break;
+        case 'e':
+            noevents = 1;
+            break;
+        case 'p':
+            p_event = 1;
+            break;
+        case 'r':
+            nodmp = 1;
+            break;
+        case 'c':
+            num_loops = strtoul(optarg, &dummy, 10);
+            break;
+        case 'w':
+            timedelay = strtoul(optarg, &dummy, 10);
+            break;
+        case 'l':
+            buf_len = strtoul(optarg, &dummy, 10);
+            break;
+        case '?':
+            return -1;
+        }
+    }
+    inv_get_sysfs_path(sysfs);
+    printf("sss:::%s\n", sysfs);
+    if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
+        printf("get chip name fail\n");
+        exit(0);
+    }
+    printf("chip_name=%s\n", chip_name);
+    if (INV_SUCCESS != inv_check_key())
+            printf("key check fail\n");
+    else
+            printf("key authenticated\n");
+
+    for (i=0; i<strlen(chip_name); i++) {
+        device_name[i] = tolower(chip_name[i]);
+    }
+    device_name[strlen(chip_name)] = '\0';
+    printf("device name: %s\n", device_name);
+
+    /* Find the device requested */
+    dev_num = find_type_by_name(device_name, "iio:device");
+    if (dev_num < 0) {
+        printf("Failed to find the %s\n", device_name);
+        ret = -ENODEV;
+        goto error_ret;
+    }
+    printf("iio device number being used is %d\n", dev_num);
+    asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
+    if (trigger_name == NULL) {
+        /*
+         * Build the trigger name. If it is device associated it's
+         * name is <device_name>_dev[n] where n matches the device
+         * number found above
+         */
+        ret = asprintf(&trigger_name,
+                   "%s-dev%d", device_name, dev_num);
+        if (ret < 0) {
+            ret = -ENOMEM;
+            goto error_ret;
+        }
+    }
+    ret = write_sysfs_int("buffer/enable", dev_dir_name, 0);
+
+	ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1);
+/*
+	ret = write_sysfs_int_and_verify("zero_motion_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("zero_motion_dur", dev_dir_name, 12);
+	ret = write_sysfs_int_and_verify("zero_motion_threshold", dev_dir_name, 13);
+
+	ret = write_sysfs_int_and_verify("motion_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("motion_dur", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("motion_threshold", dev_dir_name, 1);
+*/
+	ret = write_sysfs_int_and_verify("accel_wom_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("accel_wom_threshold", dev_dir_name, 100);
+    /* Verify the trigger exists */
+    trig_num = find_type_by_name(trigger_name, "trigger");
+    if (trig_num < 0) {
+        printf("Failed to find the trigger %s\n", trigger_name);
+        ret = -ENODEV;
+        goto error_free_triggername;
+    }
+    printf("iio trigger number being used is %d\n", trig_num);
+    /*
+     * Parse the files in scan_elements to identify what channels are
+     * present
+     */
+    ret = 0;
+    ret = enable(dev_dir_name, &infoarray, &num_channels);
+    if (ret) {
+        printf("error enable\n");
+        goto error_free_triggername;
+    }
+    if (!nodmp)
+        setup_dmp(dev_dir_name, p_event);
+
+    /*
+     * Construct the directory name for the associated buffer.
+     * As we know that the lis3l02dq has only one buffer this may
+     * be built rather than found.
+     */
+    ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_free_triggername;
+    }
+    printf("%s %s\n", dev_dir_name, trigger_name);
+
+    /* Set the device trigger to be the data rdy trigger found above */
+    ret = write_sysfs_string_and_verify("trigger/current_trigger",
+                    dev_dir_name,
+                    trigger_name);
+    if (ret < 0) {
+        printf("Failed to write current_trigger file\n");
+        goto error_free_buf_dir_name;
+    }
+    /* Setup ring buffer parameters */
+    /* length must be even number because iio_store_to_sw_ring is expecting 
+        half pointer to be equal to the read pointer, which is impossible
+        when buflen is odd number. This is actually a bug in the code */
+    ret = write_sysfs_int("length", buf_dir_name, buf_len*2);
+    if (ret < 0)
+        goto exit_here;
+    ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
+    ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
+    //ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 0);
+    if (nodmp == 0) {
+        ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1);
+    } else {
+        ret = disable_q_out(dev_dir_name, &infoarray, &num_channels);        
+        ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
+    }
+    ret = build_channel_array(dev_dir_name, &infoarray, &num_channels);
+    if (ret) {
+        printf("Problem reading scan element information\n");
+        goto exit_here;
+    }
+
+    /* Enable the buffer */
+    ret = write_sysfs_int("enable", buf_dir_name, 1);
+    if (ret < 0)
+        goto exit_here;
+    scan_size = size_from_channelarray(infoarray, num_channels);
+    data = malloc(scan_size*buf_len);
+    if (!data) {
+        ret = -ENOMEM;
+        goto exit_here;
+    }
+
+    ret = asprintf(&buffer_access,
+               "/dev/iio:device%d",
+               dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_free_data;
+    }
+    if (p_event) {
+        get_dmp_event(dev_dir_name);
+        goto error_free_buffer_access;
+    }
+    /* Attempt to open non blocking the access dev */
+    fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
+    if (fp == -1) { /*If it isn't there make the node */
+        printf("Failed to open %s\n", buffer_access);
+        ret = -errno;
+        goto error_free_buffer_access;
+    }
+    /* Wait for events 10 times */
+    for (j = 0; j < num_loops; j++) {
+        if (!noevents) {
+            struct pollfd pfd = {
+                .fd = fp,
+                .events = POLLIN,
+            };
+            poll(&pfd, 1, -1);
+            toread = 1;
+            if ((j%128)==0)
+                usleep(timedelay);
+
+        } else {
+            usleep(timedelay);
+            toread = 1;
+        }
+        read_size = read(fp,
+                 data,
+                 toread*scan_size);
+        if (read_size == -EAGAIN) {
+            printf("nothing available\n");
+            continue;
+        }
+        if (0 == p_event) {
+            for (i = 0; i < read_size/scan_size; i++)
+                process_scan(data + scan_size*i,
+                         infoarray,
+                         num_channels);
+        }
+    }
+    close(fp);
+error_free_buffer_access:
+    free(buffer_access);
+error_free_data:
+    free(data);
+exit_here:
+    /* Stop the ring buffer */
+    ret = write_sysfs_int("enable", buf_dir_name, 0);
+
+error_free_buf_dir_name:
+    free(buf_dir_name);
+error_free_triggername:
+    if (datardytrigger)
+        free(trigger_name);
+error_ret:
+    return ret;
+}
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
index 33c9eef..01d1425 100644
--- a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
index 3a055cc..ed5fbf6 100644
--- a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
@@ -18,6 +18,7 @@ HAL_DIR    = $(INV_ROOT)/software/core/HAL
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -49,20 +50,8 @@ 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
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
 
 LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
@@ -87,7 +76,7 @@ all: $(EXEC) $(MK_NAME)
 
 $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
 	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
 
 $(OBJFOLDER) :
 	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
diff --git a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
index 4f9996c..2fe2cff 100644
--- a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
+++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -22,13 +22,19 @@
 #include "ml_math_func.h"
 #include "storage_manager.h"
 #include "ml_stored_data.h"
+#include "ml_sysfs_helper.h"
 
 #ifndef ABS
 #define ABS(x)(((x) >= 0) ? (x) : -(x))
 #endif
 
+//#define DEBUG_PRINT     /* Uncomment to print Gyro & Accel read from Driver */
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
 /** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53394
+#define INV_DB_SAVE_KEY 53395
 
 #define FALSE   0
 #define TRUE    1
@@ -38,26 +44,29 @@
 #define COMPASS_PASS_STATUS_BIT 0x04
 
 typedef union {
-    long l; 
+    long l;
     int i;
 } bias_dtype;
 
-struct inv_sysfs_names_s {
+char *sysfs_names_ptr;
+
+struct sysfs_attrbs {
     char *enable;
     char *power_state;
+    char *dmp_on;
+    char *dmp_int_on;
     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"
-};
+    char *gyro_enable;
+    char *gyro_x_bias;
+    char *gyro_y_bias;
+    char *gyro_z_bias;
+    char *accel_enable;
+    char *accel_x_bias;
+    char *accel_y_bias;
+    char *accel_z_bias;
+    char *compass_enable;
+} mpu;
 
 struct inv_db_save_t {
     /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
@@ -71,46 +80,123 @@ struct inv_db_save_t {
     /** Temperature when accel bias was stored. */
     long accel_temp;
     long gyro_temp_slope[3];
+    /** Sensor Accuracy */
+    int gyro_accuracy;
+    int accel_accuracy;
+    int compass_accuracy;
 };
 
 static struct inv_db_save_t save_data;
 
-/** This function receives the data that was stored in non-volatile memory between power off */
+/** This function receives the data that was stored in non-volatile memory
+    between power off */
 static inv_error_t inv_db_load_func(const unsigned char *data)
 {
     memcpy(&save_data, data, sizeof(save_data));
     return INV_SUCCESS;
 }
 
-/** This function returns the data to be stored in non-volatile memory between power off */
+/** This function returns the data to be stored in non-volatile memory between
+    power off */
 static inv_error_t inv_db_save_func(unsigned char *data)
 {
     memcpy(data, &save_data, sizeof(save_data));
     return INV_SUCCESS;
 }
 
-int inv_sysfs_write(char *filename, long data)
+/** read a sysfs entry that represents an integer */
+int read_sysfs_int(char *filename, int *var)
 {
+    int res=0;
     FILE *fp;
-    int count;
 
-    if (!filename)
-        return -1;
+    fp = fopen(filename, "r");
+    if (fp != NULL) {
+        fscanf(fp, "%d\n", var);
+        fclose(fp);
+    } else {
+        MPL_LOGE("inv_self_test: ERR open file to read");
+        res= -1;   
+    }
+    return res;
+}
+
+/** write a sysfs entry that represents an integer */
+int write_sysfs_int(char *filename, int data)
+{
+    int res=0;
+    FILE  *fp;
+
     fp = fopen(filename, "w");
-    if (!fp)
-        return -errno;
-    count = fprintf(fp, "%ld", data);
-    fclose(fp);
-    return count;
+    if (fp!=NULL) {
+        fprintf(fp, "%d\n", data);
+        fclose(fp);
+    } else {
+        MPL_LOGE("inv_self_test: ERR open file to write");
+        res= -1;   
+    }
+    return res;
 }
 
-/**
- *  Main Self test 
- */
+int inv_init_sysfs_attributes(void)
+{
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    sysfs_names_ptr = 
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
+
+    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
+    sprintf(mpu.gyro_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias");
+    sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias");
+    sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias");
+
+    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
+    sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
+    sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
+    sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
+
+    sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable");
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+/*******************************************************************************
+ *                       M a i n  S e l f  T e s t 
+ ******************************************************************************/
 int main(int argc, char **argv)
 {
     FILE *fptr;
-    int self_test_status;
+    int self_test_status = 0;
     inv_error_t result;
     bias_dtype gyro_bias[3];
     bias_dtype accel_bias[3];
@@ -119,76 +205,133 @@ int main(int argc, char **argv)
     int axis_sign = 1;
     unsigned char *buffer;
     long timestamp;
-    int temperature=0;
+    int temperature = 0;
+    bool compass_present = TRUE;
+
+    result = inv_init_sysfs_attributes();
+    if (result)
+        return -1;
 
-    // Initialize storage manager
     inv_init_storage_manager();
 
     // Clear out data.
     memset(&save_data, 0, sizeof(save_data));
-    memset(gyro_bias,0, sizeof(gyro_bias));
-    memset(accel_bias,0, sizeof(accel_bias));
+    memset(gyro_bias, 0, sizeof(gyro_bias));
+    memset(accel_bias, 0, sizeof(accel_bias));
 
     // Register packet to be saved.
-    result = inv_register_load_store(inv_db_load_func, inv_db_save_func,
-                                     sizeof(save_data),
-                                     INV_DB_SAVE_KEY);
+    result = inv_register_load_store(
+                inv_db_load_func, inv_db_save_func,
+                sizeof(save_data), INV_DB_SAVE_KEY);
 
     // Power ON MPUxxxx chip
-    if (inv_sysfs_write(mpu.power_state, 1) <0) {
-        printf("ERR- Failed to set power state=1\n");
+    if (write_sysfs_int(mpu.power_state, 1) < 0) {
+        printf("Self-Test:ERR-Failed to set power state=1\n");
     } else {
         // Note: Driver turns on power automatically when self-test invoked
     }
 
+    // Disable Master enable 
+    if (write_sysfs_int(mpu.enable, 0) < 0) {
+        printf("Self-Test:ERR-Failed to disable master enable\n");
+    }
+
+    // Disable DMP
+    if (write_sysfs_int(mpu.dmp_on, 0) < 0) {
+        printf("Self-Test:ERR-Failed to disable DMP\n");
+    }
+
+    // Enable Accel
+    if (write_sysfs_int(mpu.accel_enable, 1) < 0) {
+        printf("Self-Test:ERR-Failed to enable accel\n");
+    }
+
+    // Enable Gyro
+    if (write_sysfs_int(mpu.gyro_enable, 1) < 0) {
+        printf("Self-Test:ERR-Failed to enable gyro\n");
+    }
+
+    // Enable Compass
+    if (write_sysfs_int(mpu.compass_enable, 1) < 0) {
+#ifdef DEBUG_PRINT
+        printf("Self-Test:ERR-Failed to enable compass\n");
+#endif
+        compass_present= FALSE;
+    }
+
     fptr = fopen(mpu.self_test, "r");
-    if (fptr != 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 (!fptr) {
+        printf("Self-Test:ERR-Couldn't invoke self-test\n");
+        result = -1;
+        goto free_sysfs_storage;
+    }
 
-        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");
-            }
+    // Invoke self-test 
+    fscanf(fptr, "%d", &self_test_status);
+	if (compass_present == TRUE) {
+        printf("Self-Test:Self test result- "
+           "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
+           (self_test_status & GYRO_PASS_STATUS_BIT),
+           (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
+           (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
+	} else {
+		printf("Self-Test:Self test result- "
+			   "Gyro passed= %x, Accel passed= %x\n",
+			   (self_test_status & GYRO_PASS_STATUS_BIT),
+			   (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1);
+	}
+    fclose(fptr);
+
+    if (self_test_status & GYRO_PASS_STATUS_BIT) {
+        // Read Gyro Bias
+        if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 ||
+            read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 ||
+            read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) {
+            memset(gyro_bias, 0, sizeof(gyro_bias));
+            printf("Self-Test:Failed to read Gyro bias\n");
         } else {
-            memset(accel_bias,0, sizeof(accel_bias));
-            printf("Self-Test:Failed Accel self-test\n");
+            save_data.gyro_accuracy = 3;
+#ifdef DEBUG_PRINT
+            printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n", 
+                   gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
+#endif
         }
+    } else {
+        printf("Self-Test:Failed Gyro 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");
-            }
-        }
-        
+    if (self_test_status & ACCEL_PASS_STATUS_BIT) {
+        // Read Accel Bias
+        if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 ||
+            read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 ||
+            read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) {
+            memset(accel_bias,0, sizeof(accel_bias));
+            printf("Self-Test:Failed to read Accel bias\n");
+        } else {
+            save_data.accel_accuracy = 3;
+#ifdef DEBUG_PRINT
+            printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n",
+                   accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
+#endif
+       }
     } else {
-        printf("Self-Test:ERR-Couldn't invoke self-test\n");
+        printf("Self-Test:Failed Accel self-test\n");
+    }
+
+    if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) {
+        printf("Self-Test:Failed Gyro and Accel self-test, "
+               "nothing left to do\n");
+        result = -1;
+        goto free_sysfs_storage;
+    }
+
+    // Read temperature
+    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");
     }
 
     // When we read gyro bias, the bias is in raw units scaled by 1000.
@@ -225,12 +368,16 @@ int main(int argc, char **argv)
     save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f);
 
 #if 1
-    printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0],
-                    save_data.accel_bias[1], save_data.accel_bias[2]);
-    printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0],
-                    save_data.gyro_bias[1], save_data.gyro_bias[2]);
-    printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp);
-    printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp);
+    printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n",
+           save_data.accel_bias[0], save_data.accel_bias[1], 
+           save_data.accel_bias[2]);
+    printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n",
+           save_data.gyro_bias[0], save_data.gyro_bias[1], 
+           save_data.gyro_bias[2]);
+    printf("Self-Test:Gyro temperature @ time stored %ld\n", 
+           save_data.gyro_temp);
+    printf("Self-Test:Accel temperature @ time stored %ld\n", 
+           save_data.accel_temp);
 #endif
 
     // Get size of packet to store.
@@ -240,25 +387,30 @@ int main(int argc, char **argv)
     buffer = (unsigned char *)malloc(packet_sz + 10);
     if (buffer == NULL) {
         printf("Self-Test:Can't allocate buffer\n");
-        return -1;
+        result = -1;
+        goto free_sysfs_storage;
     }
 
+    // Store the data
     result = inv_save_mpl_states(buffer, packet_sz);
     if (result) {
-        result= -1;
+        result = -1;
     } else {
         fptr= fopen(MLCAL_FILE, "wb+");
         if (fptr != NULL) {
             fwrite(buffer, 1, packet_sz, fptr);
             fclose(fptr);
         } else {
-            printf("Self-Test:ERR- Can't open calibration file to write - %s\n", 
+            printf("Self-Test:ERR- Can't open calibration file to write - %s\n",
                    MLCAL_FILE);
-            result= -1;
+            result = -1;
         }
     }
 
     free(buffer);
+
+free_sysfs_storage:
+    free(sysfs_names_ptr);
     return result;
 }